fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 08:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

propagate uorb contants change through all modules/drivers

Browse Source
This commit is contained in:
Thomas Gubler
2015-01-28 07:58:42 +01:00
parent 01835a51a8
commit 2d124852c1
16 changed files with 376 additions and 376 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/blinkm/blinkm.cpp
+6 -6
View File
@@ -571,7 +571,7 @@ BlinkM::led()
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -595,7 +595,7 @@ BlinkM::led()
led_color_8 = LED_BLUE;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
} else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
@@ -647,14 +647,14 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
led_color_4 = LED_OFF;
src/drivers/px4fmu/fmu.cpp
+8 -8
View File
@@ -142,11 +142,11 @@ private:
uint32_t _groups_required;
uint32_t _groups_subscribed;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _actuator_output_topic;
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
pwm_limit_t _pwm_limit;
@@ -514,7 +514,7 @@ PX4FMU::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
@@ -591,7 +591,7 @@ PX4FMU::task_main()
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
@@ -618,7 +618,7 @@ PX4FMU::task_main()
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
@@ -751,7 +751,7 @@ PX4FMU::task_main()
}
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
src/modules/commander/commander.cpp
+148 -148
View File
@@ -368,31 +368,31 @@ void print_status()
const char *armed_str;
switch (state.arming_state) {
case ARMING_STATE_INIT:
case vehicle_status_s::ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
case vehicle_status_s::ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
case vehicle_status_s::ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
case vehicle_status_s::ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
case vehicle_status_s::ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
case vehicle_status_s::ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
@@ -415,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// 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,
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
@@ -452,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state
@@ -462,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::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_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
}
}
}
@@ -525,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -550,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
unsigned int mav_goto = (cmd->param1 + 0.5f);
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status_local->nav_state = vehicle_status_s::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_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -668,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -798,41 +798,41 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
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";
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";
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
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";
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";
const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
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";
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_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
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_OFFBOARD] = "OFFBOARD";
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -853,10 +853,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.nav_state = NAVIGATION_STATE_MANUAL;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
@@ -869,7 +869,7 @@ int commander_thread_main(int argc, char *argv[])
status.data_link_lost = true;
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
// XXX for now just set sensors as initialized
@@ -1143,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[])
}
/* disable manual override for all systems that rely on electronic stabilization */
if (status.system_type == VEHICLE_TYPE_COAXIAL ||
status.system_type == VEHICLE_TYPE_HELICOPTER ||
status.system_type == VEHICLE_TYPE_TRICOPTER ||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
@@ -1159,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
@@ -1310,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(safety), safety_sub, &safety);
/* disarm if safety is now on and still armed */
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 (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
@@ -1344,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
@@ -1516,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
@@ -1524,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[])
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1535,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1549,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1662,14 +1662,14 @@ 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 (ASSIST 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.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
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_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -1688,7 +1688,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 &&
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1696,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[])
* 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) {
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1719,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
}
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
} else {
@@ -1857,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state != MAIN_STATE_MANUAL &&
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1881,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state == MAIN_STATE_ACRO ||
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1976,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[])
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
@@ -2072,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
bool set_normal_color = false;
/* set mode */
if (status_local->arming_state == ARMING_STATE_ARMED) {
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2091,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) {
/* set color */
if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
if (status_local->battery_warning == vehicle_status_s::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 */
/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
} else {
if (status_local->condition_local_position_valid) {
@@ -2149,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* if offboard is set allready by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
}
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -2166,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_ACRO);
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else {
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::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_local, MAIN_STATE_POSCTL);
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2193,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->posctl_switch != SWITCH_POS_ON) {
if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
print_reject_mode(status_local, "ALTCTL");
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::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_local, MAIN_STATE_AUTO_RTL);
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2219,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::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_local, MAIN_STATE_AUTO_LOITER);
} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2235,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_LOITER");
} else {
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2244,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2252,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@@ -2283,11 +2283,11 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {
case NAVIGATION_STATE_MANUAL:
case vehicle_status_s::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 || status.vtol_fw_permanent_stab);
@@ -2299,7 +2299,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2311,7 +2311,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2323,12 +2323,12 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RCRECOVER:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2340,7 +2340,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2352,7 +2352,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2365,7 +2365,7 @@ set_control_mode()
break;
case NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2378,7 +2378,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
@@ -2391,7 +2391,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@@ -2404,7 +2404,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = true;
break;
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
@@ -2603,7 +2603,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
@@ -2667,7 +2667,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
src/modules/commander/commander_helper.cpp
+6 -6
View File
@@ -72,16 +72,16 @@ static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
}
static int buzzer = -1;
src/modules/commander/state_machine_helper.cpp
+95 -95
View File
@@ -76,19 +76,19 @@ static const int ERROR = -1;
// 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] = {
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::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
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::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
static const char * const state_names[ARMING_STATE_MAX] = {
static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
@@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
} else {
@@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks 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) {
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
@@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
@@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
@@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
case MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
@@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_POSCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
@@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::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_OFFBOARD:
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
@@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_MAX:
case vehicle_status_s::MAIN_STATE_MAX:
default:
break;
}
@@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
} else {
switch (new_state) {
case HIL_STATE_OFF:
case vehicle_status_s::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)");
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) {
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
/* Disable publication of all attached sensors */
/* list directory */
@@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* 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:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
case vehicle_status_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
case vehicle_status_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
case vehicle_status_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
@@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
@@ -525,13 +525,13 @@ 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_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
@@ -542,37 +542,37 @@ 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_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* 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_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
@@ -580,65 +580,65 @@ 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_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::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;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
case MAIN_STATE_OFFBOARD:
case vehicle_status_s::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;
status->nav_state = vehicle_status_s::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;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_OFFBOARD;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+2 -2
View File
@@ -817,7 +817,7 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
@@ -826,7 +826,7 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+9 -9
View File
@@ -816,7 +816,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -963,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
@@ -974,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
@@ -987,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1140,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
if (launchDetector.launchDetectionEnabled() &&
@@ -1235,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
@@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
@@ -1341,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == HIL_STATE_ON);
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
}
/* check for requested subscriptions */
src/modules/mavlink/mavlink_messages.cpp
+28 -28
View File
@@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_custom_mode = 0;
/* HIL */
if (status->hil_state == HIL_STATE_ON) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
switch (status->nav_state) {
case NAVIGATION_STATE_MANUAL:
case vehicle_status_s::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;
case NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::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;
case NAVIGATION_STATE_POSCTL:
case vehicle_status_s::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;
case NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* fallthrough */
case NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -183,7 +183,7 @@ 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:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
break;
case NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::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:
case vehicle_status_s::NAVIGATION_STATE_MAX:
/* this is an unused case, ignore */
break;
@@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_custom_mode = custom_mode.data;
/* 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
if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
} else {
@@ -1438,7 +1438,7 @@ protected:
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)) {
if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@@ -2205,7 +2205,7 @@ protected:
msg.param2 = 0;
msg.param3 = 0;
/* set camera capture ON/OFF depending on arming state */
msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0;
msg.param5 = 0;
msg.param6 = 0;
msg.param7 = 0;
src/modules/mavlink/mavlink_receiver.cpp
+3 -3
View File
@@ -617,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
@@ -986,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
} else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
int64_t dt = _time_offset - offset_ns;
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
_time_offset = offset_ns;
warnx("[timesync] Hard setting offset.");
} else {
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -4
View File
@@ -768,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
@@ -998,7 +998,7 @@ MulticopterPositionControl::task_main()
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
@@ -1037,7 +1037,7 @@ MulticopterPositionControl::task_main()
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
@@ -1124,7 +1124,7 @@ MulticopterPositionControl::task_main()
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
src/modules/navigator/loiter.cpp
+1 -1
View File
@@ -82,7 +82,7 @@ Loiter::on_activation()
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_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
}
src/modules/navigator/mission.cpp
+2 -2
View File
@@ -388,7 +388,7 @@ Mission::set_mission_items()
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
set_mission_finished();
@@ -462,7 +462,7 @@ Mission::set_mission_items()
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) {
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
src/modules/navigator/mission_block.cpp
+5 -5
View File
@@ -193,25 +193,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;
default:
sp->type = SETPOINT_TYPE_POSITION;
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
}
src/modules/navigator/navigator_main.cpp
+15 -15
View File
@@ -217,7 +217,7 @@ Navigator::vehicle_status_update()
{
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
/* in case the commander is not be running */
_vstatus.arming_state = ARMING_STATE_STANDBY;
_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
}
@@ -419,25 +419,25 @@ Navigator::task_main()
/* Do stuff according to navigation state set by commander */
switch (_vstatus.nav_state) {
case NAVIGATION_STATE_MANUAL:
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
@@ -445,11 +445,11 @@ Navigator::task_main()
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
_pos_sp_triplet_published_invalid_once = false;
@@ -459,11 +459,11 @@ Navigator::task_main()
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
src/modules/sensors/sensors.cpp
+43 -43
View File
@@ -813,28 +813,28 @@ Sensors::parameters_update()
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
_rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
_rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
_rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
/* gyro offsets */
@@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced)
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
@@ -1673,17 +1673,17 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
return manual_control_setpoint_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return SWITCH_POS_MIDDLE;
return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
} else {
return SWITCH_POS_OFF;
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return SWITCH_POS_NONE;
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
@@ -1694,14 +1694,14 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
return manual_control_setpoint_s::SWITCH_POS_ON;
} else {
return SWITCH_POS_OFF;
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return SWITCH_POS_NONE;
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
@@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc()
{
static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
continue;
}
float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
@@ -1847,24 +1847,24 @@ Sensors::rc_poll()
manual.timestamp = rc_input.timestamp_last_signal;
/* limit controls */
manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {