68 #include <mathlib/mathlib.h> 70 #include <px4_platform_common/px4_config.h> 71 #include <px4_platform_common/defines.h> 72 #include <px4_platform_common/posix.h> 73 #include <px4_platform_common/shutdown.h> 74 #include <px4_platform_common/tasks.h> 75 #include <px4_platform_common/time.h> 113 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) 186 void usage(
const char *reason);
189 const uint8_t battery_warning,
const cpuload_s *cpuload_local);
217 const int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
220 case PWR_BUTTON_IDEL:
221 button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
224 case PWR_BUTTON_DOWN:
225 button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
229 button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
232 case PWR_BUTTON_REQUEST_SHUT_DOWN:
233 button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
237 PX4_ERR(
"unhandled power button state: %i", (
int)request);
245 PX4_ERR(
"power_button_state_pub not properly initialized");
256 vcmd.param1 = param1;
257 vcmd.param2 = param2;
260 vcmd.param5 = (double)NAN;
261 vcmd.param6 = (double)NAN;
269 return vcmd_pub.publish(vcmd);
275 usage(
"missing command");
279 if (!strcmp(argv[1],
"start")) {
282 PX4_INFO(
"already running");
291 unsigned constexpr max_wait_us = 1000000;
292 unsigned constexpr max_wait_steps = 2000;
296 for (i = 0; i < max_wait_steps; i++) {
297 px4_usleep(max_wait_us / max_wait_steps);
304 return !(i < max_wait_steps);
307 if (!strcmp(argv[1],
"stop")) {
310 PX4_WARN(
"already stopped");
318 PX4_INFO(
"terminated.");
325 PX4_ERR(
"not started");
329 if (!strcmp(argv[1],
"status")) {
334 if (!strcmp(argv[1],
"calibrate")) {
338 if (!strcmp(argv[2],
"mag")) {
341 }
else if (!strcmp(argv[2],
"accel")) {
344 }
else if (!strcmp(argv[2],
"gyro")) {
347 }
else if (!strcmp(argv[2],
"level")) {
350 }
else if (!strcmp(argv[2],
"esc")) {
353 }
else if (!strcmp(argv[2],
"airspeed")) {
357 PX4_ERR(
"argument %s unsupported.", argv[2]);
361 PX4_ERR(
"calibration failed, exiting.");
369 PX4_ERR(
"missing argument");
373 if (!strcmp(argv[1],
"check")) {
375 PX4_INFO(
"Preflight check: %s", preflight_check_res ?
"OK" :
"FAILED");
378 PX4_INFO(
"Prearm check: %s", prearm_check_res ?
"OK" :
"FAILED");
383 if (!strcmp(argv[1],
"arm")) {
384 bool force_arming =
false;
386 if (argc > 2 && !strcmp(argv[2],
"-f")) {
391 PX4_ERR(
"arming failed");
397 if (!strcmp(argv[1],
"disarm")) {
399 PX4_ERR(
"rejected disarm");
405 if (!strcmp(argv[1],
"takeoff")) {
416 PX4_ERR(
"arming failed");
420 PX4_ERR(
"rejecting takeoff, no position lock yet. Please retry..");
423 return (ret ? 0 : 1);
426 if (!strcmp(argv[1],
"land")) {
429 return (ret ? 0 : 1);
432 if (!strcmp(argv[1],
"transition")) {
435 (
float)(status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
436 vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
437 vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC));
439 return (ret ? 0 : 1);
442 if (!strcmp(argv[1],
"mode")) {
444 uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
446 if (!strcmp(argv[2],
"manual")) {
447 new_main_state = commander_state_s::MAIN_STATE_MANUAL;
449 }
else if (!strcmp(argv[2],
"altctl")) {
450 new_main_state = commander_state_s::MAIN_STATE_ALTCTL;
452 }
else if (!strcmp(argv[2],
"posctl")) {
453 new_main_state = commander_state_s::MAIN_STATE_POSCTL;
455 }
else if (!strcmp(argv[2],
"auto:mission")) {
456 new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
458 }
else if (!strcmp(argv[2],
"auto:loiter")) {
459 new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
461 }
else if (!strcmp(argv[2],
"auto:rtl")) {
462 new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
464 }
else if (!strcmp(argv[2],
"acro")) {
465 new_main_state = commander_state_s::MAIN_STATE_ACRO;
467 }
else if (!strcmp(argv[2],
"offboard")) {
468 new_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
470 }
else if (!strcmp(argv[2],
"stabilized")) {
471 new_main_state = commander_state_s::MAIN_STATE_STAB;
473 }
else if (!strcmp(argv[2],
"rattitude")) {
474 new_main_state = commander_state_s::MAIN_STATE_RATTITUDE;
476 }
else if (!strcmp(argv[2],
"auto:takeoff")) {
477 new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
479 }
else if (!strcmp(argv[2],
"auto:land")) {
480 new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
482 }
else if (!strcmp(argv[2],
"auto:precland")) {
483 new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
486 PX4_ERR(
"argument %s unsupported.", argv[2]);
490 PX4_ERR(
"mode change failed");
496 PX4_ERR(
"missing argument");
500 if (!strcmp(argv[1],
"lockdown")) {
503 usage(
"not enough arguments, missing [on, off]");
508 strcmp(argv[2],
"off") ? 2.0
f : 0.0
f , 0.0f);
510 return (ret ? 0 : 1);
513 usage(
"unrecognized command");
538 arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
540 run_preflight_checks,
541 mavlink_log_pub_local,
547 mavlink_log_info(mavlink_log_pub_local,
"%s by %s", arm ?
"ARMED" :
"DISARMED", armedBy);
557 ModuleParams(nullptr),
558 _failure_detector(this)
564 internal_state.
main_state = commander_state_s::MAIN_STATE_MANUAL;
565 status.
nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
567 status.
arming_state = vehicle_status_s::ARMING_STATE_INIT;
591 unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
595 case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
603 if ((((uint32_t)cmd.
param2) & 1) > 0) {
605 status_flags, &internal_state);
608 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
611 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
616 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
621 case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
622 uint8_t base_mode = (uint8_t)cmd.
param1;
623 uint8_t custom_main_mode = (uint8_t)cmd.
param2;
624 uint8_t custom_sub_mode = (uint8_t)cmd.
param3;
638 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
642 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
647 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
651 if (custom_sub_mode > 0) {
654 switch (custom_sub_mode) {
656 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
662 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
672 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state);
676 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags,
681 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state);
685 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags,
690 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags,
701 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
707 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state);
711 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state);
715 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
720 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
727 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
733 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
737 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
741 main_ret =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
747 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
750 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
759 case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
763 if (static_cast<int>(cmd.
param1 + 0.5f) != 0 && static_cast<int>(cmd.
param1 + 0.5f) != 1) {
768 bool cmd_arms = (
static_cast<int>(cmd.
param1 + 0.5f) == 1);
771 const bool enforce = (
static_cast<int>(roundf(cmd.
param2)) == 21196);
776 if (armed_local->
armed) {
787 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
793 status.
arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
797 if ((!status_local->
hil_state) != vehicle_status_s::HIL_STATE_ON
800 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
804 const bool throttle_above_low = (sp_man.
z > 0.1f);
805 const bool throttle_above_center = (sp_man.
z > 0.6f);
807 if (cmd_arms && throttle_above_center &&
808 (status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
809 status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
811 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
815 if (cmd_arms && throttle_above_low &&
816 (status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
817 status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
818 status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
819 status_local->
nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) {
821 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
830 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
833 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
846 case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
849 warnx(
"forcing lockdown (motors off)");
851 }
else if (cmd.
param1 > 0.5f) {
854 warnx(
"forcing failsafe (termination)");
856 if ((
int)cmd.
param2 <= 0) {
858 warnx(
"reset all non-flighttermination failsafe commands");
864 warnx(
"disabling failsafe and lockdown");
867 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
871 case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
872 bool use_current = cmd.
param1 > 0.5f;
877 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
880 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
884 const double lat = cmd.
param5;
885 const double lon = cmd.
param6;
886 const float alt = cmd.
param7;
888 if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
900 home.manual_home =
true;
901 home.valid_alt =
true;
902 home.valid_hpos =
true;
908 home.z = -(alt - local_pos.
ref_alt);
913 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
916 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
920 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
926 case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: {
928 static main_state_t main_state_pre_offboard = commander_state_s::MAIN_STATE_MANUAL;
930 if (internal_state.
main_state != commander_state_s::MAIN_STATE_OFFBOARD) {
931 main_state_pre_offboard = internal_state.
main_state;
935 res =
main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
954 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
957 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
962 case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
967 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
971 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
976 case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
980 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
982 }
else if (internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
983 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
987 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
992 case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
996 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1000 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1005 case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
1007 status_flags, &internal_state)) {
1009 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1013 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1018 case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
1020 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1033 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1036 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1047 case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
1050 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
1056 case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
1061 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1064 cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1069 case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
1073 case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
1074 case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
1075 case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
1076 case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
1077 case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
1078 case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
1079 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
1080 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
1081 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
1082 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
1083 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
1084 case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
1085 case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
1086 case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
1087 case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
1088 case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
1089 case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
1090 case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
1091 case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
1092 case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
1093 case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
1094 case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
1095 case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
1096 case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
1097 case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
1098 case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
1099 case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
1100 case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
1101 case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
1102 case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
1103 case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
1110 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
1114 if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
1126 return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1129 if (_param_com_mot_test_en.get() != 1) {
1130 return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1135 test_motor.motor_number = (int)(cmd.
param1 + 0.5f) - 1;
1136 int throttle_type = (int)(cmd.
param2 + 0.5f);
1138 if (throttle_type != 0) {
1139 return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
1142 int motor_count = (int)(cmd.
param5 + 0.5);
1144 if (motor_count > 1) {
1145 return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
1148 test_motor.action = test_motor_s::ACTION_RUN;
1153 test_motor.value = -1.f;
1156 test_motor.timeout_ms = (int)(cmd.
param4 * 1000.f + 0.5f);
1159 if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
1160 test_motor.timeout_ms = 3000;
1163 test_motor.driver_instance = 0;
1166 return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1182 if ((gpos.
eph <= _param_com_home_h_t.get()) && (gpos.
epv <= _param_com_home_v_t.get())) {
1191 home.lat = gpos.
lat;
1192 home.lon = gpos.
lon;
1193 home.valid_hpos =
true;
1195 home.alt = gpos.
alt;
1196 home.valid_alt =
true;
1202 home.yaw = lpos.
yaw;
1204 home.manual_home =
false;
1230 home.valid_alt =
true;
1243 bool sensor_fail_tune_played =
false;
1244 bool arm_tune_played =
false;
1245 bool was_landed =
true;
1246 bool was_falling =
false;
1247 bool was_armed =
false;
1285 pthread_t commander_low_prio_thread;
1289 PX4_WARN(
"LED init failed");
1293 PX4_WARN(
"Buzzer init failed");
1301 button_state.
event = 0xff;
1304 power_button_state_sub.copy(&button_state);
1308 PX4_ERR(
"Failed to register power notification callback");
1325 int stick_off_counter = 0;
1326 int stick_on_counter = 0;
1328 bool status_changed =
true;
1329 bool param_init_forced =
true;
1345 land_detector.
landed =
true;
1354 int32_t system_type;
1355 param_get(_param_sys_type, &system_type);
1359 status.
vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
1365 status.
vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
1368 status.
vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
1382 int32_t rc_in_off = 0;
1384 param_get(_param_rc_in_off, &rc_in_off);
1386 int32_t arm_switch_is_button = 0;
1387 param_get(_param_arm_switch_is_button, &arm_switch_is_button);
1389 int32_t arm_without_gps_param = 0;
1390 param_get(_param_arm_without_gps, &arm_without_gps_param);
1393 int32_t arm_mission_required_param = 0;
1394 param_get(_param_arm_mission_required, &arm_mission_required_param);
1398 int32_t arm_auth_required_param = 0;
1399 param_get(_param_arm_auth_required, &arm_auth_required_param);
1403 int32_t arm_escs_checks_required_param = 0;
1404 param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
1411 int32_t rc_arm_hyst = 100;
1412 param_get(_param_rc_arm_hyst, &rc_arm_hyst);
1415 int32_t geofence_action = 0;
1416 int32_t flight_uuid = 0;
1417 int32_t airmode = 0;
1418 int32_t rc_map_arm_switch = 0;
1421 int32_t rc_override = 0;
1423 int32_t takeoff_complete_act = 0;
1426 float ef_throttle_thres = 1.0f;
1427 float ef_current2throttle_thres = 0.0f;
1428 float ef_time_thres = 1000.0f;
1429 uint64_t timestamp_engine_healthy = 0;
1432 bool failsafe_old =
false;
1434 bool have_taken_off_since_arming =
false;
1437 pthread_attr_t commander_low_prio_attr;
1438 pthread_attr_init(&commander_low_prio_attr);
1439 pthread_attr_setstacksize(&commander_low_prio_attr, PX4_STACK_ADJUSTED(3000));
1443 struct sched_param param;
1444 (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m);
1447 param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
1448 (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m);
1452 pthread_attr_destroy(&commander_low_prio_attr);
1459 while (!should_exit()) {
1466 if (params_updated || param_init_forced) {
1476 if (
param_get(_param_sys_type, &system_type) !=
OK) {
1477 PX4_ERR(
"failed getting new system type");
1489 status.
vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
1491 }
else if (is_fixed) {
1494 }
else if (is_ground) {
1495 status.
vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
1507 int32_t comp_id = 0;
1508 param_get(_param_component_id, &comp_id);
1513 status_changed =
true;
1517 param_get(_param_rc_in_off, &rc_in_off);
1519 param_get(_param_rc_arm_hyst, &rc_arm_hyst);
1521 param_get(_param_rc_override, &rc_override);
1525 param_get(_param_ef_throttle_thres, &ef_throttle_thres);
1526 param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
1527 param_get(_param_ef_time_thres, &ef_time_thres);
1528 param_get(_param_geofence_action, &geofence_action);
1529 param_get(_param_flight_uuid, &flight_uuid);
1531 param_get(_param_arm_switch_is_button, &arm_switch_is_button);
1533 param_get(_param_arm_without_gps, &arm_without_gps_param);
1536 param_get(_param_arm_mission_required, &arm_mission_required_param);
1540 param_get(_param_arm_auth_required, &arm_auth_required_param);
1544 param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
1557 param_get(_param_takeoff_finished_action, &takeoff_complete_act);
1564 param_get(_param_rc_map_arm_switch, &rc_map_arm_switch);
1566 if (airmode == 2 && rc_map_arm_switch == 0) {
1573 param_init_forced =
false;
1580 if (power_button_state_sub.updated()) {
1582 power_button_state_sub.copy(&button_state);
1584 if (button_state.
event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
1585 px4_shutdown_request(
false,
false);
1589 sp_man_sub.update(&sp_man);
1593 if (system_power_sub.updated()) {
1595 system_power_sub.copy(&system_power);
1616 px4_shutdown_request(
true,
false);
1622 if (safety_sub.updated()) {
1623 bool previous_safety_off = safety.
safety_off;
1625 if (safety_sub.copy(&safety)) {
1628 if (armed.
armed && (status.
hil_state == vehicle_status_s::HIL_STATE_OFF)
1635 status_changed =
true;
1649 status_changed =
true;
1655 if (vtol_vehicle_status_sub.updated()) {
1657 vtol_vehicle_status_sub.copy(&vtol_status);
1665 vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
1670 vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
1672 status_changed =
true;
1677 status_changed =
true;
1682 status_changed =
true;
1687 status_changed =
true;
1690 const bool should_soft_stop = (status.
vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
1692 if (armed.
soft_stop != should_soft_stop) {
1694 status_changed =
true;
1699 if (esc_status_sub.updated()) {
1703 esc_status_sub.copy(&esc_status);
1710 if (land_detector_sub.updated()) {
1711 land_detector_sub.copy(&land_detector);
1715 if (was_landed != land_detector.
landed) {
1716 if (land_detector.
landed) {
1721 have_taken_off_since_arming =
true;
1732 if (was_falling != land_detector.
freefall) {
1739 was_landed = land_detector.
landed;
1740 was_falling = land_detector.
freefall;
1748 if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
1750 if (_param_com_disarm_land.get() > 0 && have_taken_off_since_arming) {
1754 }
else if (_param_com_disarm_preflight.get() > 0 && !have_taken_off_since_arming) {
1782 }
else if (internal_state.
main_state != commander_state_s::MAIN_STATE_AUTO_RTL
1783 && internal_state.
main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
1784 && internal_state.
main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
1789 cpuload_sub.update(&cpuload);
1796 while (subsys_sub.update(&info)) {
1798 status_changed =
true;
1826 if (mission_result_ok) {
1830 status_changed =
true;
1839 (prev_mission_instance_count != mission_result.
instance_count)) {
1845 }
else if (mission_result.
warning) {
1858 geofence_result_sub.update(&geofence_result);
1861 const bool geofence_action_enabled = geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
1862 const bool not_in_battery_failsafe =
_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
1865 && geofence_action_enabled
1866 && not_in_battery_failsafe) {
1871 switch (geofence_result.geofence_action) {
1872 case (geofence_result_s::GF_ACTION_NONE) : {
1877 case (geofence_result_s::GF_ACTION_WARN) : {
1882 case (geofence_result_s::GF_ACTION_LOITER) : {
1891 case (geofence_result_s::GF_ACTION_RTL) : {
1900 case (geofence_result_s::GF_ACTION_TERMINATE) : {
1901 warnx(
"Flight termination because of geofence");
1904 status_changed =
true;
1913 const bool in_loiter_mode = internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
1914 const bool manual_loiter_switch_on = sp_man.
loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
1916 if (!in_loiter_mode || manual_loiter_switch_on) {
1922 const bool in_rtl_mode = internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
1923 const bool manual_return_switch_on = sp_man.
return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
1925 if (!in_rtl_mode || manual_return_switch_on) {
1941 const bool not_in_low_battery_reaction =
_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
1943 const bool manual_mode_before_geofence =
1950 const bool in_auto_mode =
1951 internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
1952 internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
1953 internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
1954 internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
1956 if (rc_override != 0 && is_rotary_wing && not_in_low_battery_reaction
1976 status_changed =
true;
1977 static bool flight_termination_printed =
false;
1979 if (!flight_termination_printed) {
1981 flight_termination_printed =
true;
1997 status_changed =
true;
2004 status_changed =
true;
2010 const bool in_armed_state = (status.
arming_state == vehicle_status_s::ARMING_STATE_ARMED);
2011 const bool arm_switch_or_button_mapped = sp_man.
arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
2012 const bool arm_button_pressed = arm_switch_is_button == 1
2013 && sp_man.
arm_switch == manual_control_setpoint_s::SWITCH_POS_ON;
2021 && !arm_switch_or_button_mapped;
2022 const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 &&
2024 sp_man.
arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF;
2026 if (in_armed_state &&
2028 (status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || land_detector.
landed) &&
2029 (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
2031 const bool manual_thrust_mode = internal_state.
main_state == commander_state_s::MAIN_STATE_MANUAL
2032 || internal_state.
main_state == commander_state_s::MAIN_STATE_ACRO
2033 || internal_state.
main_state == commander_state_s::MAIN_STATE_STAB
2034 || internal_state.
main_state == commander_state_s::MAIN_STATE_RATTITUDE;
2035 const bool rc_wants_disarm = (stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst)
2036 || arm_switch_to_disarm_transition;
2038 if (rc_wants_disarm && (land_detector.
landed || manual_thrust_mode)) {
2044 stick_off_counter++;
2046 }
else if (!(arm_switch_is_button == 1 && sp_man.
arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
2048 stick_off_counter = 0;
2056 && !arm_switch_or_button_mapped;
2059 const bool in_arming_grace_period = last_disarmed_timestamp != 0 &&
hrt_elapsed_time(&last_disarmed_timestamp) < 5_s;
2060 const bool arm_switch_to_arm_transition = arm_switch_is_button == 0 &&
2062 sp_man.
arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
2063 (sp_man.
z < 0.1f || in_arming_grace_period);
2065 if (!in_armed_state &&
2067 (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
2068 if ((stick_on_counter == rc_arm_hyst && stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
2075 if ((internal_state.
main_state != commander_state_s::MAIN_STATE_MANUAL)
2076 && (internal_state.
main_state != commander_state_s::MAIN_STATE_ACRO)
2077 && (internal_state.
main_state != commander_state_s::MAIN_STATE_STAB)
2078 && (internal_state.
main_state != commander_state_s::MAIN_STATE_ALTCTL)
2079 && (internal_state.
main_state != commander_state_s::MAIN_STATE_POSCTL)
2080 && (internal_state.
main_state != commander_state_s::MAIN_STATE_RATTITUDE)
2085 geofence_action == geofence_result_s::GF_ACTION_RTL) {
2088 }
else if (status.
arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
2090 !in_arming_grace_period ,
2102 }
else if (!(arm_switch_is_button == 1 && sp_man.
arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
2104 stick_on_counter = 0;
2131 status_changed =
true;
2139 if (sp_man.
kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2143 status_changed =
true;
2147 }
else if (sp_man.
kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
2150 status_changed =
true;
2162 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER,
true,
true,
false, status);
2163 status_changed =
true;
2172 if (actuator_controls_sub.updated()) {
2180 actuator_controls_sub.copy(&actuator_controls);
2182 const float throttle = actuator_controls.
control[actuator_controls_s::INDEX_THROTTLE];
2185 if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres))
2191 if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres)
2195 status_changed =
true;
2197 PX4_ERR(
"Engine Failure");
2198 set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,
true,
true,
false, status);
2208 status_changed =
true;
2217 if (internal_state.
main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
2224 if ((takeoff_complete_act == 1) && mission_available) {
2225 main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state);
2228 main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2242 main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2247 if (cmd_sub.updated()) {
2254 if (
handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) {
2255 status_changed =
true;
2262 if (failure_detector_updated) {
2268 status_changed =
true;
2273 failure_detector_updated &&
2280 status_changed =
true;
2297 if ((!was_armed || (was_landed && !land_detector.
landed)) &&
2308 float home_dist_xy = -1.0f;
2309 float home_dist_z = -1.0f;
2311 local_position.
x, local_position.
y, local_position.
z,
2312 &home_dist_xy, &home_dist_z);
2314 if ((home_dist_xy > local_position.
eph * 2.0f) || (home_dist_z > local_position.
epv * 2.0f)) {
2336 if (was_armed != armed.
armed) {
2337 status_changed =
true;
2341 param_set(_param_flight_uuid, &flight_uuid);
2346 was_armed = armed.
armed;
2363 if (nav_state_changed) {
2367 if (status.
failsafe != failsafe_old) {
2368 status_changed =
true;
2388 switch ((PrearmedMode)_param_com_prearm_mode.get()) {
2389 case PrearmedMode::DISABLED:
2394 case PrearmedMode::ALWAYS:
2402 case PrearmedMode::SAFETY_BUTTON:
2436 arm_tune_played =
true;
2439 (status.
hil_state != vehicle_status_s::HIL_STATE_ON) &&
2444 }
else if ((status.
hil_state != vehicle_status_s::HIL_STATE_ON) &&
2460 if (arm_tune_played) {
2464 arm_tune_played =
false;
2474 sensor_fail_tune_played =
true;
2475 status_changed =
true;
2482 if (blink_state > 0) {
2484 if (blink_state == 2) {
2494 status_changed =
false;
2498 have_taken_off_since_arming =
false;
2509 int ret = pthread_join(commander_low_prio_thread,
nullptr);
2512 warn(
"join failed: %d", ret);
2548 bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
2550 if (*valid_out != valid_new) {
2551 *valid_out = valid_new;
2558 bool changed,
const uint8_t battery_warning,
const cpuload_s *cpuload_local)
2562 bool overload = (cpuload_local->
load > 0.95f) || (cpuload_local->
ram_usage > 0.98f);
2564 if (overload_start == 0 && overload) {
2567 }
else if (!overload) {
2573 uint8_t led_mode = led_control_s::MODE_OFF;
2574 uint8_t led_color = led_control_s::COLOR_WHITE;
2575 bool set_normal_color =
false;
2577 uint64_t overload_warn_delay = (status_local->
arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
2580 if (overload && (
hrt_elapsed_time(&overload_start) > overload_warn_delay)) {
2581 led_mode = led_control_s::MODE_BLINK_FAST;
2582 led_color = led_control_s::COLOR_PURPLE;
2584 }
else if (status_local->
arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
2585 led_mode = led_control_s::MODE_ON;
2586 set_normal_color =
true;
2589 led_mode = led_control_s::MODE_BLINK_FAST;
2590 led_color = led_control_s::COLOR_RED;
2592 }
else if (status_local->
arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
2593 led_mode = led_control_s::MODE_BREATHE;
2594 set_normal_color =
true;
2597 led_mode = led_control_s::MODE_BREATHE;
2598 set_normal_color =
true;
2600 }
else if (status_local->
arming_state == vehicle_status_s::ARMING_STATE_INIT) {
2602 led_mode = led_control_s::MODE_OFF;
2605 led_mode = led_control_s::MODE_BLINK_NORMAL;
2606 led_color = led_control_s::COLOR_RED;
2609 if (set_normal_color) {
2612 led_color = led_control_s::COLOR_PURPLE;
2614 }
else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
2615 led_color = led_control_s::COLOR_AMBER;
2617 }
else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
2618 led_color = led_control_s::COLOR_RED;
2622 led_color = led_control_s::COLOR_GREEN;
2625 led_color = led_control_s::COLOR_BLUE;
2630 if (led_mode != led_control_s::MODE_OFF) {
2639 BOARD_INDICATE_ARMED_STATE(actuator_armed->
armed);
2641 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) 2644 if (actuator_armed->
armed) {
2646 BOARD_ARMED_LED_OFF();
2649 BOARD_ARMED_STATE_LED_TOGGLE();
2653 BOARD_ARMED_STATE_LED_OFF();
2656 BOARD_ARMED_LED_ON();
2660 BOARD_ARMED_LED_OFF();
2664 BOARD_ARMED_STATE_LED_TOGGLE();
2668 BOARD_ARMED_LED_OFF();
2672 BOARD_ARMED_STATE_LED_TOGGLE();
2681 BOARD_OVERLOAD_LED_TOGGLE();
2685 BOARD_OVERLOAD_LED_OFF();
2725 const bool first_time_rc = (_last_sp_man.
timestamp == 0);
2727 const bool some_switch_changed =
2740 const bool should_evaluate_rc_mode_switch = first_time_rc
2741 || altitude_got_valid
2744 || (rc_values_updated && some_switch_changed);
2746 if (!should_evaluate_rc_mode_switch) {
2753 && (internal_state.
main_state == commander_state_s::MAIN_STATE_MANUAL ||
2754 internal_state.
main_state == commander_state_s::MAIN_STATE_ALTCTL ||
2755 internal_state.
main_state == commander_state_s::MAIN_STATE_POSCTL ||
2756 internal_state.
main_state == commander_state_s::MAIN_STATE_ACRO ||
2757 internal_state.
main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
2758 internal_state.
main_state == commander_state_s::MAIN_STATE_STAB)) {
2761 _last_sp_man.
x = sp_man.
x;
2762 _last_sp_man.
y = sp_man.
y;
2763 _last_sp_man.
z = sp_man.
z;
2764 _last_sp_man.
r = sp_man.
r;
2778 if (sp_man.
offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2779 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
2792 if (sp_man.
return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2793 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state);
2799 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2811 if (sp_man.
loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2812 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2823 if (sp_man.
mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
2825 if (sp_man.
mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
2826 warnx(
"m slot overflow");
2848 if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
2851 new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2860 if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) {
2863 new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2872 if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) {
2875 new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2884 if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
2887 new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2896 if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) {
2899 new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2908 if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
2911 new_mode = commander_state_s::MAIN_STATE_POSCTL;
2920 if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
2923 new_mode = commander_state_s::MAIN_STATE_ALTCTL;
2932 if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
2935 new_mode = commander_state_s::MAIN_STATE_STAB;
2944 if (new_mode == commander_state_s::MAIN_STATE_STAB) {
2947 new_mode = commander_state_s::MAIN_STATE_MANUAL;
2963 case manual_control_setpoint_s::SWITCH_POS_NONE:
2967 case manual_control_setpoint_s::SWITCH_POS_OFF:
2968 if (sp_man.
stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
2969 sp_man.
man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
2974 if (sp_man.
acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2978 if (status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.
is_vtol) {
2979 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state);
2982 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
2985 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
2988 }
else if (sp_man.
rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2991 if (status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
2992 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state);
2995 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
2999 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3007 if (sp_man.
man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
3008 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3010 }
else if (sp_man.
acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
3011 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state);
3013 }
else if (sp_man.
rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
3014 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state);
3016 }
else if (sp_man.
stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
3017 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
3019 }
else if (sp_man.
man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
3021 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3025 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
3032 case manual_control_setpoint_s::SWITCH_POS_MIDDLE:
3033 if (sp_man.
posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
3034 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
3044 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
3050 if (sp_man.
posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
3055 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3059 case manual_control_setpoint_s::SWITCH_POS_ON:
3060 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state);
3069 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
3076 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
3083 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
3090 res =
main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3128 bool *validity_changed)
3130 const bool was_valid = *valid_state;
3131 bool valid = was_valid;
3134 if (land_detector.
landed) {
3138 const bool data_stale = ((
hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s)
3139 || (data_timestamp_us == 0));
3140 const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
3142 const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
3145 if (level_check_pass) {
3148 const int64_t probation_time_new = *probation_time_us -
hrt_elapsed_time(last_fail_time_us);
3166 const int64_t probation_time_new = *probation_time_us +
hrt_elapsed_time(last_fail_time_us) *
3167 _param_com_pos_fs_gain.get();
3174 if (was_valid != valid) {
3175 *validity_changed =
true;
3176 *valid_state = valid;
3190 control_mode.flag_armed = armed.
armed;
3195 case vehicle_status_s::NAVIGATION_STATE_MANUAL:
3196 control_mode.flag_control_manual_enabled =
true;
3201 case vehicle_status_s::NAVIGATION_STATE_STAB:
3202 control_mode.flag_control_manual_enabled =
true;
3203 control_mode.flag_control_rates_enabled =
true;
3204 control_mode.flag_control_attitude_enabled =
true;
3207 case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
3208 control_mode.flag_control_manual_enabled =
true;
3209 control_mode.flag_control_rates_enabled =
true;
3210 control_mode.flag_control_attitude_enabled =
true;
3211 control_mode.flag_control_rattitude_enabled =
true;
3214 case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
3215 control_mode.flag_control_manual_enabled =
true;
3216 control_mode.flag_control_rates_enabled =
true;
3217 control_mode.flag_control_attitude_enabled =
true;
3218 control_mode.flag_control_altitude_enabled =
true;
3219 control_mode.flag_control_climb_rate_enabled =
true;
3222 case vehicle_status_s::NAVIGATION_STATE_POSCTL:
3223 control_mode.flag_control_manual_enabled =
true;
3224 control_mode.flag_control_rates_enabled =
true;
3225 control_mode.flag_control_attitude_enabled =
true;
3226 control_mode.flag_control_altitude_enabled =
true;
3227 control_mode.flag_control_climb_rate_enabled =
true;
3232 case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
3233 case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
3235 control_mode.flag_external_manual_override_ok =
false;
3238 case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
3239 case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
3240 case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
3241 case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
3242 case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
3243 case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
3244 case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
3245 case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
3246 control_mode.flag_control_auto_enabled =
true;
3247 control_mode.flag_control_rates_enabled =
true;
3248 control_mode.flag_control_attitude_enabled =
true;
3249 control_mode.flag_control_altitude_enabled =
true;
3250 control_mode.flag_control_climb_rate_enabled =
true;
3255 case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
3256 control_mode.flag_control_rates_enabled =
true;
3257 control_mode.flag_control_attitude_enabled =
true;
3258 control_mode.flag_control_climb_rate_enabled =
true;
3261 case vehicle_status_s::NAVIGATION_STATE_ACRO:
3262 control_mode.flag_control_manual_enabled =
true;
3263 control_mode.flag_control_rates_enabled =
true;
3266 case vehicle_status_s::NAVIGATION_STATE_DESCEND:
3267 control_mode.flag_control_auto_enabled =
false;
3268 control_mode.flag_control_rates_enabled =
true;
3269 control_mode.flag_control_attitude_enabled =
true;
3270 control_mode.flag_control_climb_rate_enabled =
true;
3273 case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
3275 control_mode.flag_control_termination_enabled =
true;
3278 case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
3282 control_mode.flag_control_offboard_enabled =
true;
3288 control_mode.flag_control_rates_enabled =
3297 control_mode.flag_control_attitude_enabled = !offboard_control_mode.
ignore_attitude ||
3303 control_mode.flag_control_yawrate_override_enabled =
3309 control_mode.flag_control_rattitude_enabled =
false;
3314 control_mode.flag_control_velocity_enabled = (!offboard_control_mode.
ignore_velocity ||
3316 !control_mode.flag_control_acceleration_enabled;
3318 control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.
ignore_velocity ||
3319 !offboard_control_mode.
ignore_position) && !control_mode.flag_control_acceleration_enabled;
3322 !control_mode.flag_control_acceleration_enabled;
3324 control_mode.flag_control_altitude_enabled = (!offboard_control_mode.
ignore_velocity ||
3325 !offboard_control_mode.
ignore_position) && !control_mode.flag_control_acceleration_enabled;
3330 case vehicle_status_s::NAVIGATION_STATE_ORBIT:
3331 control_mode.flag_control_manual_enabled =
false;
3332 control_mode.flag_control_auto_enabled =
false;
3333 control_mode.flag_control_rates_enabled =
true;
3334 control_mode.flag_control_attitude_enabled =
true;
3335 control_mode.flag_control_rattitude_enabled =
false;
3336 control_mode.flag_control_altitude_enabled =
true;
3337 control_mode.flag_control_climb_rate_enabled =
true;
3340 control_mode.flag_control_acceleration_enabled =
false;
3341 control_mode.flag_control_termination_enabled =
false;
3354 return (status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ||
3392 case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
3395 case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
3399 case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
3403 case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
3407 case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
3419 command_ack.command = cmd.
command;
3420 command_ack.result = (uint8_t)result;
3425 command_ack_pub.
publish(command_ack);
3431 px4_prctl(PR_SET_NAME,
"commander_low_prio", px4_getpid());
3440 px4_pollfd_struct_t fds[1];
3442 fds[0].fd = cmd_sub;
3443 fds[0].events = POLLIN;
3447 int pret =
px4_poll(&fds[0], (
sizeof(fds) /
sizeof(fds[0])), 1000);
3451 warn(
"commander: poll error %d, %d", pret, errno);
3454 }
else if (pret != 0) {
3461 if (cmd.
command == vehicle_command_s::VEHICLE_CMD_DO_SET_MODE ||
3462 cmd.
command == vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM ||
3463 cmd.
command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF ||
3464 cmd.
command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO ||
3465 cmd.
command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
3473 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
3474 if (((
int)(cmd.
param1)) == 0) {
3475 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3482 if (((
int)(cmd.
param1)) == 1) {
3483 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3486 px4_shutdown_request(
true,
false);
3488 }
else if (((
int)(cmd.
param1)) == 2) {
3489 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3492 px4_shutdown_request(
false,
false);
3494 }
else if (((
int)(cmd.
param1)) == 3) {
3495 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3498 px4_shutdown_request(
true,
true);
3501 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3505 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3510 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
3512 int calib_ret = PX4_ERROR;
3519 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3526 if ((
int)(cmd.
param1) == 1) {
3528 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3531 }
else if ((
int)(cmd.
param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
3532 (
int)(cmd.
param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
3533 (
int)(cmd.
param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
3537 }
else if ((
int)(cmd.
param2) == 1) {
3539 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3542 }
else if ((
int)(cmd.
param3) == 1) {
3544 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3546 }
else if ((
int)(cmd.
param4) == 1) {
3548 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3554 }
else if ((
int)(cmd.
param4) == 2) {
3556 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3559 }
else if ((
int)(cmd.
param5) == 1) {
3561 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3564 }
else if ((
int)(cmd.
param5) == 2) {
3566 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3569 }
else if ((
int)(cmd.
param6) == 1 || (
int)(cmd.
param6) == 2) {
3572 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3575 }
else if ((
int)(cmd.
param7) == 1) {
3577 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3580 }
else if ((
int)(cmd.
param4) == 0) {
3588 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3593 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
3598 if (calib_ret ==
OK) {
3614 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
3616 if (((
int)(cmd.
param1)) == 0) {
3621 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3635 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
3638 }
else if (((
int)(cmd.
param1)) == 1) {
3643 px4_usleep(1000000);
3650 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3664 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
3667 }
else if (((
int)(cmd.
param1)) == 2) {
3674 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3680 case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
3682 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3705 PX4_WARN(
"%s\n", reason);
3713 _task_id = px4_task_spawn_cmd(
"commander",
3715 SCHED_PRIORITY_DEFAULT + 40,
3717 (px4_main_t)&run_trampoline,
3718 (
char *
const *)argv);
3733 if (argc >= 2 && !strcmp(argv[1],
"-h")) {
3743 status.
hil_state = vehicle_status_s::HIL_STATE_ON;
3753 if (mission.
count > 0) {
3758 PX4_ERR(
"reading mission state failed");
3794 switch (telemetry.
type) {
3795 case telemetry_status_s::LINK_TYPE_USB:
3800 case telemetry_status_s::LINK_TYPE_IRIDIUM: {
3810 status_changed =
true;
3821 case telemetry_status_s::MAV_TYPE_GCS:
3827 status_changed =
true;
3843 case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
3849 status_changed =
true;
3856 if (telemetry.
remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
3866 status_changed =
true;
3888 status_changed =
true;
3899 status_changed =
true;
3942 status_changed =
true;
3958 status_changed =
true;
3972 && battery.connected
3992 if ((battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) &&
shutdown_if_allowed()) {
3996 int ret_val = px4_shutdown_request(
false,
false);
4002 while (1) { px4_usleep(1); }
4031 const bool mag_fault = (estimator_status.
control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
4033 if (!mag_fault_prev && mag_fault) {
4039 const bool reliant_on_opt_flow = ((estimator_status.
control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
4043 const bool operator_controlled_position = (internal_state.
main_state == commander_state_s::MAIN_STATE_POSCTL);
4058 if (run_quality_checks && status.
vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
4060 if (status.
arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
4066 }
else if (land_detector.
landed) {
4073 const bool sufficient_speed = (lpos.
vx * lpos.
vx + lpos.
vy * lpos.
vy > 25.0f);
4080 if (sufficient_time || sufficient_speed) {
4085 if (innovation_pass) {
4102 if (run_quality_checks) {
4140 bool force_update =
false;
4142 if (commander_state_s::MAIN_STATE_OFFBOARD) {
4143 if (offboard_control_mode.
timestamp == 0) {
4145 force_update =
true;
4163 status_changed =
true;
4168 if (offboard_control_mode.
timestamp != 0 &&
4173 status_changed =
true;
4179 status_changed =
true;
4195 status_changed =
true;
4204 char esc_fail_msg[50];
4205 esc_fail_msg[0] =
'\0';
4207 int online_bitmask = (1 << esc_status.
esc_count) - 1;
4224 for (
int index = 0; index < esc_status.
esc_count; index++) {
4226 snprintf(esc_fail_msg + strlen(esc_fail_msg),
sizeof(esc_fail_msg) - strlen(esc_fail_msg),
"ESC%d ", index + 1);
4227 esc_fail_msg[
sizeof(esc_fail_msg) - 1] =
'\0';
4241 PX4_INFO(
"%s", reason);
4244 PRINT_MODULE_DESCRIPTION(
4247 The commander module contains the state machine for mode switching and failsafe behavior. 4250 PRINT_MODULE_USAGE_NAME("commander",
"system");
4251 PRINT_MODULE_USAGE_COMMAND(
"start");
4252 PRINT_MODULE_USAGE_PARAM_FLAG(
'h',
"Enable HIL mode",
true);
4253 PRINT_MODULE_USAGE_COMMAND_DESCR(
"calibrate",
"Run sensor calibration");
4254 PRINT_MODULE_USAGE_ARG(
"mag|accel|gyro|level|esc|airspeed",
"Calibration type",
false);
4255 PRINT_MODULE_USAGE_COMMAND_DESCR(
"check",
"Run preflight checks");
4256 PRINT_MODULE_USAGE_COMMAND(
"arm");
4257 PRINT_MODULE_USAGE_PARAM_FLAG(
'f',
"Force arming (do not run preflight checks)",
true);
4258 PRINT_MODULE_USAGE_COMMAND(
"disarm");
4259 PRINT_MODULE_USAGE_COMMAND(
"takeoff");
4260 PRINT_MODULE_USAGE_COMMAND(
"land");
4261 PRINT_MODULE_USAGE_COMMAND_DESCR(
"transition",
"VTOL transition");
4262 PRINT_MODULE_USAGE_COMMAND_DESCR(
"mode",
"Change flight mode");
4263 PRINT_MODULE_USAGE_ARG(
"manual|acro|offboard|stabilized|rattitude|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
4264 "Flight mode",
false);
4265 PRINT_MODULE_USAGE_COMMAND(
"lockdown");
4266 PRINT_MODULE_USAGE_ARG(
"off",
"Turn lockdown off",
true);
4267 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
#define VEHICLE_TYPE_FIXED_WING
uORB::Publication< commander_state_s > _commander_state_pub
#define mavlink_log_info(_pub, _text,...)
Send a mavlink info message (not printed to console).
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning, const low_battery_action_t low_battery_action)
uORB::Subscription _battery_sub
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
static bool _last_condition_local_altitude_valid
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
uORB::SubscriptionData< vehicle_local_position_s > _local_position_sub
void battery_status_check()
void tune_neutral(bool use_buzzer)
Blink white LED and play neutral tune (if use_buzzer == true).
static float min_stick_change
float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, float x_next, float y_next, float z_next, float *dist_xy, float *dist_z)
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
static int custom_command(int argc, char *argv[])
FailureDetector _failure_detector
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
uint32_t control_mode_flags
const int64_t POSVEL_PROBATION_MIN
minimum probation duration (usec)
static struct vehicle_status_s status
State machine helper functions definitions.
uint8_t data_link_lost_counter
void tune_mission_fail(bool use_buzzer)
uORB::Publication< test_motor_s > _test_motor_pub
Definition of a mission consisting of mission items.
static constexpr uint64_t OFFBOARD_TIMEOUT
bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic)
uint64_t nav_state_timestamp
bool condition_power_input_valid
hrt_abstime _datalink_last_heartbeat_avoidance_system
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition of geo / math functions to perform geodesic calculations.
bool set_home_position_alt_only()
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
bool _print_avoidance_msg_once
bool is_fixed_wing(const struct vehicle_status_s *current_status)
uORB::Publication< actuator_armed_s > _armed_pub
static struct vehicle_status_flags_s status_flags
bool _geofence_warning_action_on
bool stabilization_required()
uint8_t failure_detector_status
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL
static constexpr float STICK_ON_OFF_LIMIT
int do_level_calibration(orb_advert_t *mavlink_log_pub)
int main(int argc, char **argv)
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
void usage(const char *reason)
Print the correct usage.
Barometer calibration routine.
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX]
bool publish(const T &data)
Publish the struct.
void arm_auth_update(hrt_abstime now, bool param_update)
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status)
#define CBRK_AIRSPD_CHK_KEY
hrt_abstime _last_gpos_fail_time_us
Last time that the global position validity recovery check failed (usec)
int info()
Print a little info about the driver.
Airspeed sensor calibration routine.
static struct commander_state_s internal_state
hrt_abstime _datalink_last_heartbeat_gcs
bool circuit_breaker_engaged_posfailure_check
bool circuit_breaker_engaged_power_check
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
hrt_abstime _last_lpos_fail_time_us
Last time that the local position validity recovery check failed (usec)
void set_tune_override(int tune)
uORB::SubscriptionData< vehicle_global_position_s > _global_position_sub
uint8_t remote_component_id
static bool _last_condition_global_position_valid
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]
static manual_control_setpoint_s sp_man
the current manual control setpoint
uORB::Subscription _telemetry_status_sub
bool circuit_breaker_engaged_enginefailure_check
void tune_positive(bool use_buzzer)
Blink green LED and play positive tune (if use_buzzer == true).
bool safety_switch_available
bool circuit_breaker_engaged_usb_check
bool condition_global_position_valid
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, uORB::PublicationQueued< vehicle_command_ack_s > &command_ack_pub, bool *changed)
bool condition_home_position_valid
High-resolution timer with callouts and timekeeping.
hrt_abstime _time_at_takeoff
last time we were on the ground
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
bool vtol_fw_permanent_stab
#define mavlink_and_console_log_info(_pub, _text,...)
Send a mavlink emergency message and print to console.
hrt_abstime _gpos_probation_time_us
bool offboard_control_loss_timeout
#define CBRK_SUPPLY_CHK_KEY
__EXPORT int commander_main(int argc, char *argv[])
The daemon app only briefly exists to start the background job.
bool circuit_breaker_engaged_gpsfailure_check
bool condition_local_velocity_valid
bool _nav_test_failed
true if the post takeoff navigation test has failed
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
hrt_abstime _high_latency_datalink_heartbeat
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
bool condition_battery_healthy
static bool _skip_pos_accuracy_check
uORB::Subscription _parameter_update_sub
void update_control_mode()
int orb_subscribe(const struct orb_metadata *meta)
bool _nav_test_passed
true if the post takeoff navigation test has passed
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot)
Runs a preflight check on all sensors to see if they are properly calibrated and healthy.
static struct safety_s safety
static uint64_t rc_signal_lost_timestamp
void * commander_low_prio_loop(void *arg)
Loop that runs at a lower rate and priority for calibration and parameter tasks.
void print_reject_mode(const char *msg)
transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed)
void reset_posvel_validity(bool *changed)
__BEGIN_DECLS void led_init()
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void estimator_check(bool *status_changed)
bool vtol_transition_failsafe
void print_reject_arm(const char *msg)
#define COMMANDER_MONITORING_LOOPSPERMSEC
hrt_abstime _datalink_last_heartbeat_onboard_controller
bool _onboard_controller_lost
bool _avoidance_system_lost
bool rc_signal_found_once
Commander helper functions definitions.
bool is_vtol_tailsitter(const struct vehicle_status_s *current_status)
__EXPORT ssize_t dm_read(dm_item_t item, unsigned index, void *buf, size_t count)
Retrieve from the data manager file.
bool publish(const T &data)
Publish the struct.
hrt_abstime _lvel_probation_time_us
bool condition_system_sensors_initialized
bool condition_local_altitude_valid
static Commander * instantiate(int argc, char *argv[])
static hrt_abstime commander_boot_timestamp
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
bool rc_calibration_valid
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS
static struct actuator_armed_s armed
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool _avoidance_system_status_change
uint8_t remote_system_status
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
bool offboard_control_signal_lost
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT
wait for hotplug sensors to come online for upto 8 seconds
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
bool condition_auto_mission_available
static bool _last_condition_local_position_valid
hrt_abstime _high_latency_datalink_lost
bool high_latency_data_link_lost
static int print_usage(const char *reason=nullptr)
uORB::SubscriptionData< offboard_control_mode_s > _offboard_control_mode_sub
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
enum LOW_BAT_ACTION low_battery_action_t
__EXPORT int param_save_default(void)
Save parameters to the default file.
uORB::SubscriptionData< mission_result_s > _mission_result_sub
bool is_rotary_wing(const struct vehicle_status_s *current_status)
void tune_mission_ok(bool use_buzzer)
bool is_vtol(const struct vehicle_status_s *current_status)
bool vtol_transition_failure
static volatile bool thread_should_exit
daemon exit flag
bool is_ground_rover(const struct vehicle_status_s *current_status)
systemlib::Hysteresis _auto_disarm_killed
bool updated()
Check if there is a new update.
static orb_advert_t mavlink_log_pub
transition_result_t set_main_state(const vehicle_status_s &status, bool *changed)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
systemlib::Hysteresis _auto_disarm_landed
Driver for the PX4 audio alarm port, /dev/tone_alarm.
void data_link_check(bool &status_changed)
Checks the status of all available data links and handles switching between different system telemetr...
void offboard_control_update(bool &status_changed)
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
static bool last_overload
bool avoidance_system_valid
bool _flight_termination_triggered
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
bool is_safe(const safety_s &safety, const actuator_armed_s &armed)
unsigned handle_command_motor_test(const vehicle_command_s &cmd)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
uORB::PublicationData< home_position_s > _home_pub
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL
Contains helper functions to efficiently set the system health flags from commander and preflight che...
static uint8_t arm_requirements
void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed)
void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
void get_circuit_breaker_params()
bool _geofence_violated_prev
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
int _last_esc_online_flags
void esc_status_check(const esc_status_s &esc_status)
uORB::Publication< vehicle_status_s > _status_pub
Definition of esc calibration.
offboard_loss_rc_actions_t
const int64_t POSVEL_PROBATION_MAX
maximum probation duration (usec)
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, const uint8_t arm_requirements)
bool circuit_breaker_flight_termination_disabled
bool offboard_control_set_by_command
static float _eph_threshold_adj
maximum allowable horizontal position uncertainty after adjustment for flight condition ...
static uint8_t main_state_before_rtl
static float actuator_controls[output_max]
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
bool shutdown_if_allowed()
uORB::SubscriptionData< estimator_status_s > _estimator_status_sub
Remote Control calibration routine.
bool condition_calibration_enabled
Definition of accelerometer calibration.
static uint64_t last_print_mode_reject_time
position_nav_loss_actions_t
uint8_t getStatus() const
static orb_advert_t power_button_state_pub
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy)
__EXPORT void param_reset_all(void)
Reset all parameters to their default values.
transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed)
static bool preflight_check(bool report)
int orb_unadvertise(orb_advert_t handle)
#define mavlink_log_emergency(_pub, _text,...)
Send a mavlink emergency message and print to console.
static struct cpuload_s cpuload
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local)
bool ignore_acceleration_force
bool condition_escs_error
bool circuit_breaker_engaged_airspd_check
transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state)
uORB::Publication< vehicle_status_flags_s > _vehicle_status_flags_pub
static volatile bool thread_running
daemon status flag
hrt_abstime _lpos_probation_time_us
__EXPORT ssize_t dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count)
Write to the data manager file.
#define CBRK_ENGINEFAIL_KEY
bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state, orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, const offboard_loss_rc_actions_t offb_loss_rc_act, const position_nav_loss_actions_t posctl_nav_loss_act)
Check failsafe and main status and set navigation status for navigator accordingly.
bool avoidance_system_required
transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot)
bool update(void *dst)
Update the struct.
static uint8_t _last_sp_man_arm_switch
#define CBRK_FLIGHTTERM_KEY
static manual_control_setpoint_s _last_sp_man
the manual control setpoint valid at the last mode switch
static bool send_vehicle_command(uint16_t cmd, float param1=NAN, float param2=NAN)
bool update(const vehicle_status_s &vehicle_status)
Check if flight is safely possible if not prevent it and inform the user.
uORB::Publication< vehicle_control_mode_s > _control_mode_pub
bool condition_system_hotplug_timeout
hrt_abstime _last_lvel_fail_time_us
Last time that the local velocity validity recovery check failed (usec)
#define mavlink_log_warning(_pub, _text,...)
Send a mavlink warning message and print to console.
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
Initializes the map transformation given by the argument and sets the timestamp to now...
uORB::Subscription _iridiumsbd_status_sub
bool condition_local_position_valid
#define CBRK_VELPOSERR_KEY
bool copy(void *dst)
Copy the struct.
void tune_failsafe(bool use_buzzer)
static void answer_command(const vehicle_command_s &cmd, unsigned result, uORB::PublicationQueued< vehicle_command_ack_s > &command_ack_pub)
__EXPORT int param_load_default(void)
Load parameters from the default parameter file.
static unsigned int leds_counter
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
static struct vtol_vehicle_status_s vtol_status
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
uint8_t _datalink_last_status_avoidance_system
void tune_negative(bool use_buzzer)
Blink red LED and play negative tune (if use_buzzer == true).
void tune_home_set(bool use_buzzer)
bool set_home_position()
This function initializes the home position an altitude of the vehicle.
hrt_abstime _time_last_innov_pass
last time velocity or position innovations passed
static int task_spawn(int argc, char *argv[])