PX4 Firmware
PX4 Autopilot Software http://px4.io
Commander.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file commander.cpp
36  *
37  * Main state machine / business logic
38  *
39  * @TODO This application is currently in a rewrite process. Main changes:
40  * - Calibration routines are moved into the event system
41  * - Commander is rewritten as class
42  * - State machines will be model driven
43  */
44 
45 #include "Commander.hpp"
46 
47 /* commander module headers */
52 #include "airspeed_calibration.h"
53 #include "baro_calibration.h"
54 #include "calibration_routines.h"
55 #include "commander_helper.h"
56 #include "esc_calibration.h"
57 #include "gyro_calibration.h"
58 #include "mag_calibration.h"
59 #include "px4_custom_mode.h"
60 #include "rc_calibration.h"
61 #include "state_machine_helper.h"
62 
63 /* PX4 headers */
64 #include <dataman/dataman.h>
65 #include <drivers/drv_hrt.h>
66 #include <drivers/drv_tone_alarm.h>
67 #include <lib/ecl/geo/geo.h>
68 #include <mathlib/mathlib.h>
69 #include <navigator/navigation.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>
77 #include <systemlib/mavlink_log.h>
78 
79 #include <math.h>
80 #include <float.h>
81 #include <cstring>
82 
85 #include <uORB/topics/cpuload.h>
89 #include <uORB/topics/mission.h>
91 #include <uORB/topics/safety.h>
97 #include <uORB/topics/esc_status.h>
98 
99 typedef enum VEHICLE_MODE_FLAG {
100  VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
101  VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
102  VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
103  VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
104  VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
105  VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
106  VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
107  VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
110 
111 /* Decouple update interval and hysteresis counters, all depends on intervals */
112 static constexpr uint64_t COMMANDER_MONITORING_INTERVAL = 10_ms;
113 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
114 
115 static constexpr float STICK_ON_OFF_LIMIT = 0.9f;
116 
117 static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms;
118 static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sensors to come online for upto 8 seconds */
119 static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms;
120 static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms;
121 
122 /* Mavlink log uORB handle */
123 static orb_advert_t mavlink_log_pub = nullptr;
125 
126 /* flags */
127 static volatile bool thread_should_exit = false; /**< daemon exit flag */
128 static volatile bool thread_running = false; /**< daemon status flag */
129 
131 
132 static unsigned int leds_counter;
133 /* To remember when last notification was sent */
134 static uint64_t last_print_mode_reject_time = 0;
135 
136 static float min_stick_change = 0.25f;
137 
138 static struct vehicle_status_s status = {};
139 static struct actuator_armed_s armed = {};
140 static struct safety_s safety = {};
141 static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM];
143 
144 static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
145 
146 static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
147 static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch
148 static uint8_t _last_sp_man_arm_switch = 0;
149 
151 static struct cpuload_s cpuload = {};
152 
153 static bool last_overload = false;
154 
156 
157 static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost
158 
160 
164 
166 
167 static float _eph_threshold_adj =
168  INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
169 static bool _skip_pos_accuracy_check = false;
170 
171 /**
172  * The daemon app only briefly exists to start
173  * the background job. The stack size assigned in the
174  * Makefile does only apply to this management task.
175  *
176  * The actual stack size should be set in the call
177  * to task_create().
178  *
179  * @ingroup apps
180  */
181 extern "C" __EXPORT int commander_main(int argc, char *argv[]);
182 
183 /**
184  * Print the correct usage.
185  */
186 void usage(const char *reason);
187 
188 void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
189  const uint8_t battery_warning, const cpuload_s *cpuload_local);
190 
192 
193 void print_reject_mode(const char *msg);
194 
195 void print_reject_arm(const char *msg);
196 
197 void print_status();
198 
199 bool shutdown_if_allowed();
200 
201 transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy);
202 
203 /**
204  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
205  */
206 void *commander_low_prio_loop(void *arg);
207 
208 static void answer_command(const vehicle_command_s &cmd, unsigned result,
210 
211 static int power_button_state_notification_cb(board_power_button_state_notification_e request)
212 {
213  // Note: this can be called from IRQ handlers, so we publish a message that will be handled
214  // on the main thread of commander.
215  power_button_state_s button_state{};
216  button_state.timestamp = hrt_absolute_time();
217  const int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
218 
219  switch (request) {
220  case PWR_BUTTON_IDEL:
221  button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
222  break;
223 
224  case PWR_BUTTON_DOWN:
225  button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
226  break;
227 
228  case PWR_BUTTON_UP:
229  button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
230  break;
231 
232  case PWR_BUTTON_REQUEST_SHUT_DOWN:
233  button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
234  break;
235 
236  default:
237  PX4_ERR("unhandled power button state: %i", (int)request);
238  return ret;
239  }
240 
241  if (power_button_state_pub != nullptr) {
242  orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state);
243 
244  } else {
245  PX4_ERR("power_button_state_pub not properly initialized");
246  }
247 
248  return ret;
249 }
250 
251 static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
252 {
253  vehicle_command_s vcmd{};
254 
255  vcmd.timestamp = hrt_absolute_time();
256  vcmd.param1 = param1;
257  vcmd.param2 = param2;
258  vcmd.param3 = NAN;
259  vcmd.param4 = NAN;
260  vcmd.param5 = (double)NAN;
261  vcmd.param6 = (double)NAN;
262  vcmd.param7 = NAN;
263  vcmd.command = cmd;
264  vcmd.target_system = status.system_id;
265  vcmd.target_component = status.component_id;
266 
267  uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
268 
269  return vcmd_pub.publish(vcmd);
270 }
271 
272 int commander_main(int argc, char *argv[])
273 {
274  if (argc < 2) {
275  usage("missing command");
276  return 1;
277  }
278 
279  if (!strcmp(argv[1], "start")) {
280 
281  if (thread_running) {
282  PX4_INFO("already running");
283  /* this is not an error */
284  return 0;
285  }
286 
287  thread_should_exit = false;
288 
289  Commander::main(argc, argv);
290 
291  unsigned constexpr max_wait_us = 1000000;
292  unsigned constexpr max_wait_steps = 2000;
293 
294  unsigned i;
295 
296  for (i = 0; i < max_wait_steps; i++) {
297  px4_usleep(max_wait_us / max_wait_steps);
298 
299  if (thread_running) {
300  break;
301  }
302  }
303 
304  return !(i < max_wait_steps);
305  }
306 
307  if (!strcmp(argv[1], "stop")) {
308 
309  if (!thread_running) {
310  PX4_WARN("already stopped");
311  return 0;
312  }
313 
314  thread_should_exit = true;
315 
316  Commander::main(argc, argv);
317 
318  PX4_INFO("terminated.");
319 
320  return 0;
321  }
322 
323  /* commands needing the app to run below */
324  if (!thread_running) {
325  PX4_ERR("not started");
326  return 1;
327  }
328 
329  if (!strcmp(argv[1], "status")) {
330  print_status();
331  return 0;
332  }
333 
334  if (!strcmp(argv[1], "calibrate")) {
335  if (argc > 2) {
336  int calib_ret = OK;
337 
338  if (!strcmp(argv[2], "mag")) {
339  calib_ret = do_mag_calibration(&mavlink_log_pub);
340 
341  } else if (!strcmp(argv[2], "accel")) {
343 
344  } else if (!strcmp(argv[2], "gyro")) {
345  calib_ret = do_gyro_calibration(&mavlink_log_pub);
346 
347  } else if (!strcmp(argv[2], "level")) {
349 
350  } else if (!strcmp(argv[2], "esc")) {
351  calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
352 
353  } else if (!strcmp(argv[2], "airspeed")) {
355 
356  } else {
357  PX4_ERR("argument %s unsupported.", argv[2]);
358  }
359 
360  if (calib_ret) {
361  PX4_ERR("calibration failed, exiting.");
362  return 1;
363 
364  } else {
365  return 0;
366  }
367 
368  } else {
369  PX4_ERR("missing argument");
370  }
371  }
372 
373  if (!strcmp(argv[1], "check")) {
374  bool preflight_check_res = Commander::preflight_check(true);
375  PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
376 
377  bool prearm_check_res = PreFlightCheck::preArmCheck(&mavlink_log_pub, status_flags, safety, arm_requirements);
378  PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
379 
380  return 0;
381  }
382 
383  if (!strcmp(argv[1], "arm")) {
384  bool force_arming = false;
385 
386  if (argc > 2 && !strcmp(argv[2], "-f")) {
387  force_arming = true;
388  }
389 
390  if (TRANSITION_CHANGED != arm_disarm(true, !force_arming, &mavlink_log_pub, "command line")) {
391  PX4_ERR("arming failed");
392  }
393 
394  return 0;
395  }
396 
397  if (!strcmp(argv[1], "disarm")) {
398  if (TRANSITION_DENIED == arm_disarm(false, true, &mavlink_log_pub, "command line")) {
399  PX4_ERR("rejected disarm");
400  }
401 
402  return 0;
403  }
404 
405  if (!strcmp(argv[1], "takeoff")) {
406 
407  bool ret = false;
408 
409  /* see if we got a home position */
410  if (status_flags.condition_local_position_valid) {
411 
412  if (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "command line")) {
413  ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
414 
415  } else {
416  PX4_ERR("arming failed");
417  }
418 
419  } else {
420  PX4_ERR("rejecting takeoff, no position lock yet. Please retry..");
421  }
422 
423  return (ret ? 0 : 1);
424  }
425 
426  if (!strcmp(argv[1], "land")) {
427  bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND);
428 
429  return (ret ? 0 : 1);
430  }
431 
432  if (!strcmp(argv[1], "transition")) {
433 
434  const bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_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));
438 
439  return (ret ? 0 : 1);
440  }
441 
442  if (!strcmp(argv[1], "mode")) {
443  if (argc > 2) {
444  uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
445 
446  if (!strcmp(argv[2], "manual")) {
447  new_main_state = commander_state_s::MAIN_STATE_MANUAL;
448 
449  } else if (!strcmp(argv[2], "altctl")) {
450  new_main_state = commander_state_s::MAIN_STATE_ALTCTL;
451 
452  } else if (!strcmp(argv[2], "posctl")) {
453  new_main_state = commander_state_s::MAIN_STATE_POSCTL;
454 
455  } else if (!strcmp(argv[2], "auto:mission")) {
456  new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
457 
458  } else if (!strcmp(argv[2], "auto:loiter")) {
459  new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
460 
461  } else if (!strcmp(argv[2], "auto:rtl")) {
462  new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
463 
464  } else if (!strcmp(argv[2], "acro")) {
465  new_main_state = commander_state_s::MAIN_STATE_ACRO;
466 
467  } else if (!strcmp(argv[2], "offboard")) {
468  new_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
469 
470  } else if (!strcmp(argv[2], "stabilized")) {
471  new_main_state = commander_state_s::MAIN_STATE_STAB;
472 
473  } else if (!strcmp(argv[2], "rattitude")) {
474  new_main_state = commander_state_s::MAIN_STATE_RATTITUDE;
475 
476  } else if (!strcmp(argv[2], "auto:takeoff")) {
477  new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
478 
479  } else if (!strcmp(argv[2], "auto:land")) {
480  new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
481 
482  } else if (!strcmp(argv[2], "auto:precland")) {
483  new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
484 
485  } else {
486  PX4_ERR("argument %s unsupported.", argv[2]);
487  }
488 
489  if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) {
490  PX4_ERR("mode change failed");
491  }
492 
493  return 0;
494 
495  } else {
496  PX4_ERR("missing argument");
497  }
498  }
499 
500  if (!strcmp(argv[1], "lockdown")) {
501 
502  if (argc < 3) {
503  usage("not enough arguments, missing [on, off]");
504  return 1;
505  }
506 
507  bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
508  strcmp(argv[2], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f);
509 
510  return (ret ? 0 : 1);
511  }
512 
513  usage("unrecognized command");
514  return 1;
515 }
516 
518 {
519  PX4_INFO("arming: %s", arming_state_names[status.arming_state]);
520 }
521 
523 {
524  return TRANSITION_DENIED != arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
525  &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, arm_requirements,
527 }
528 
529 transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local,
530  const char *armedBy)
531 {
533 
534  // Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will
535  // output appropriate error messages if the state cannot transition.
536  arming_res = arming_state_transition(&status,
537  safety,
538  arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
539  &armed,
540  run_preflight_checks,
541  mavlink_log_pub_local,
542  &status_flags,
545 
546  if (arming_res == TRANSITION_CHANGED) {
547  mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy);
548 
549  } else if (arming_res == TRANSITION_DENIED) {
550  tune_negative(true);
551  }
552 
553  return arming_res;
554 }
555 
557  ModuleParams(nullptr),
558  _failure_detector(this)
559 {
560  _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
561 
562  // We want to accept RC inputs as default
563  status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
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;
568 
569  /* mark all signals lost as long as they haven't been found */
570  status.rc_signal_lost = true;
571  status_flags.offboard_control_signal_lost = true;
572  status.data_link_lost = true;
573 
574  status_flags.condition_power_input_valid = true;
575  status_flags.rc_calibration_valid = true;
576 
577  status_flags.avoidance_system_valid = false;
578 }
579 
580 bool
582  uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
583 {
584  /* only handle commands that are meant to be handled by this system and component */
585  if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
586  && (cmd.target_component != 0))) { // component_id 0: valid for all components
587  return false;
588  }
589 
590  /* result of the command */
591  unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
592 
593  /* request to set different system mode */
594  switch (cmd.command) {
595  case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
596 
597  // Just switch the flight mode here, the navigator takes care of
598  // doing something sensible with the coordinates. Its designed
599  // to not require navigator and command to receive / process
600  // the data at the exact same time.
601 
602  // Check if a mode switch had been requested
603  if ((((uint32_t)cmd.param2) & 1) > 0) {
604  transition_result_t main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER,
605  status_flags, &internal_state);
606 
607  if ((main_ret != TRANSITION_DENIED)) {
608  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
609 
610  } else {
611  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
612  mavlink_log_critical(&mavlink_log_pub, "Reposition command rejected");
613  }
614 
615  } else {
616  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
617  }
618  }
619  break;
620 
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;
625 
627 
629 
630  // We ignore base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED because
631  // the command VEHICLE_CMD_COMPONENT_ARM_DISARM should be used
632  // instead according to the latest mavlink spec.
633 
634  if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
635  /* use autopilot-specific mode */
636  if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
637  /* MANUAL */
638  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
639 
640  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
641  /* ALTCTL */
642  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
643 
644  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
645  /* POSCTL */
646  reset_posvel_validity(changed);
647  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
648 
649  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
650  /* AUTO */
651  if (custom_sub_mode > 0) {
652  reset_posvel_validity(changed);
653 
654  switch (custom_sub_mode) {
656  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
657  &internal_state);
658  break;
659 
661  if (status_flags.condition_auto_mission_available) {
662  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
663  &internal_state);
664 
665  } else {
666  main_ret = TRANSITION_DENIED;
667  }
668 
669  break;
670 
672  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state);
673  break;
674 
676  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags,
677  &internal_state);
678  break;
679 
681  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state);
682  break;
683 
685  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags,
686  &internal_state);
687  break;
688 
690  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags,
691  &internal_state);
692  break;
693 
694  default:
695  main_ret = TRANSITION_DENIED;
696  mavlink_log_critical(&mavlink_log_pub, "Unsupported auto mode");
697  break;
698  }
699 
700  } else {
701  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
702  &internal_state);
703  }
704 
705  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
706  /* ACRO */
707  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &internal_state);
708 
709  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
710  /* RATTITUDE */
711  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &internal_state);
712 
713  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
714  /* STABILIZED */
715  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
716 
717  } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
718  reset_posvel_validity(changed);
719  /* OFFBOARD */
720  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
721  }
722 
723  } else {
724  /* use base mode */
725  if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
726  /* AUTO */
727  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
728  &internal_state);
729 
730  } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
731  if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
732  /* POSCTL */
733  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
734 
735  } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
736  /* STABILIZED */
737  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
738 
739  } else {
740  /* MANUAL */
741  main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
742  }
743  }
744  }
745 
746  if ((arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) {
747  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
748 
749  } else {
750  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
751 
752  if (arming_ret == TRANSITION_DENIED) {
753  mavlink_log_critical(&mavlink_log_pub, "Arming command rejected");
754  }
755  }
756  }
757  break;
758 
759  case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
760 
761  // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
762  // for logic state parameters
763  if (static_cast<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) {
764  mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1);
765 
766  } else {
767 
768  bool cmd_arms = (static_cast<int>(cmd.param1 + 0.5f) == 1);
769 
770  // Arm/disarm is enforced when param2 is set to a magic number.
771  const bool enforce = (static_cast<int>(roundf(cmd.param2)) == 21196);
772 
773  if (!enforce) {
774  if (!land_detector.landed && !is_ground_rover(&status)) {
775  if (cmd_arms) {
776  if (armed_local->armed) {
777  mavlink_log_warning(&mavlink_log_pub, "Arming denied! Already armed");
778 
779  } else {
780  mavlink_log_critical(&mavlink_log_pub, "Arming denied! Not landed");
781  }
782 
783  } else {
784  mavlink_log_critical(&mavlink_log_pub, "Disarming denied! Not landed");
785  }
786 
787  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
788  break;
789  }
790 
791  // Flick to inair restore first if this comes from an onboard system
792  if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id) {
793  status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
794 
795  } else {
796  // Refuse to arm if preflight checks have failed
797  if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON
798  && !status_flags.condition_system_sensors_initialized) {
799  mavlink_log_critical(&mavlink_log_pub, "Arming denied! Preflight checks have failed");
800  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
801  break;
802  }
803 
804  const bool throttle_above_low = (sp_man.z > 0.1f);
805  const bool throttle_above_center = (sp_man.z > 0.6f);
806 
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)) {
810  mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not centered");
811  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
812  break;
813  }
814 
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)) {
820  mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not zero");
821  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
822  break;
823  }
824  }
825  }
826 
827  transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, "Arm/Disarm component command");
828 
829  if (arming_res == TRANSITION_DENIED) {
830  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
831 
832  } else {
833  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
834 
835  /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
836  if (cmd_arms && (arming_res == TRANSITION_CHANGED) &&
838 
840  }
841  }
842  }
843  }
844  break;
845 
846  case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
847  if (cmd.param1 > 1.5f) {
848  armed_local->lockdown = true;
849  warnx("forcing lockdown (motors off)");
850 
851  } else if (cmd.param1 > 0.5f) {
852  //XXX update state machine?
853  armed_local->force_failsafe = true;
854  warnx("forcing failsafe (termination)");
855 
856  if ((int)cmd.param2 <= 0) {
857  /* reset all commanded failure modes */
858  warnx("reset all non-flighttermination failsafe commands");
859  }
860 
861  } else {
862  armed_local->force_failsafe = false;
863  armed_local->lockdown = false;
864  warnx("disabling failsafe and lockdown");
865  }
866 
867  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
868  }
869  break;
870 
871  case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
872  bool use_current = cmd.param1 > 0.5f;
873 
874  if (use_current) {
875  /* use current position */
876  if (set_home_position()) {
877  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
878 
879  } else {
880  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
881  }
882 
883  } else {
884  const double lat = cmd.param5;
885  const double lon = cmd.param6;
886  const float alt = cmd.param7;
887 
888  if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
889  const vehicle_local_position_s &local_pos = _local_position_sub.get();
890 
891  if (local_pos.xy_global && local_pos.z_global) {
892  /* use specified position */
893  home_position_s home{};
894  home.timestamp = hrt_absolute_time();
895 
896  home.lat = lat;
897  home.lon = lon;
898  home.alt = alt;
899 
900  home.manual_home = true;
901  home.valid_alt = true;
902  home.valid_hpos = true;
903 
904  // update local projection reference including altitude
905  struct map_projection_reference_s ref_pos;
906  map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
907  map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
908  home.z = -(alt - local_pos.ref_alt);
909 
910  /* mark home position as set */
911  status_flags.condition_home_position_valid = _home_pub.update(home);
912 
913  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
914 
915  } else {
916  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
917  }
918 
919  } else {
920  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
921  }
922  }
923  }
924  break;
925 
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;
929 
930  if (internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) {
931  main_state_pre_offboard = internal_state.main_state;
932  }
933 
934  if (cmd.param1 > 0.5f) {
935  res = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
936 
937  if (res == TRANSITION_DENIED) {
938  print_reject_mode("OFFBOARD");
939  status_flags.offboard_control_set_by_command = false;
940 
941  } else {
942  /* Set flag that offboard was set via command, main state is not overridden by rc */
943  status_flags.offboard_control_set_by_command = true;
944  }
945 
946  } else {
947  /* If the mavlink command is used to enable or disable offboard control:
948  * switch back to previous mode when disabling */
949  res = main_state_transition(*status_local, main_state_pre_offboard, status_flags, &internal_state);
950  status_flags.offboard_control_set_by_command = false;
951  }
952 
953  if (res == TRANSITION_DENIED) {
954  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
955 
956  } else {
957  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
958  }
959  }
960  break;
961 
962  case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
963  /* switch to RTL which ends the mission */
964  if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
965  &internal_state)) {
966  mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch");
967  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
968 
969  } else {
970  mavlink_log_critical(&mavlink_log_pub, "Return to launch denied");
971  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
972  }
973  }
974  break;
975 
976  case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
977  /* ok, home set, use it to take off */
978  if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags,
979  &internal_state)) {
980  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
981 
982  } else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
983  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
984 
985  } else {
986  mavlink_log_critical(&mavlink_log_pub, "Takeoff denied! Please disarm and retry");
987  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
988  }
989  }
990  break;
991 
992  case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
993  if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
994  &internal_state)) {
995  mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
996  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
997 
998  } else {
999  mavlink_log_critical(&mavlink_log_pub, "Landing denied! Please land manually");
1000  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1001  }
1002  }
1003  break;
1004 
1005  case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
1006  if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND,
1007  status_flags, &internal_state)) {
1008  mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing");
1009  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1010 
1011  } else {
1012  mavlink_log_critical(&mavlink_log_pub, "Precision landing denied! Please land manually");
1013  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1014  }
1015  }
1016  break;
1017 
1018  case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
1019 
1020  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1021 
1022  // check if current mission and first item are valid
1023  if (status_flags.condition_auto_mission_available) {
1024 
1025  // requested first mission item valid
1026  if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
1027 
1028  // switch to AUTO_MISSION and ARM
1029  if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
1030  &internal_state))
1031  && (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "Mission start command"))) {
1032 
1033  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1034 
1035  } else {
1036  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1037  mavlink_log_critical(&mavlink_log_pub, "Mission start denied");
1038  }
1039  }
1040 
1041  } else {
1042  mavlink_log_critical(&mavlink_log_pub, "Mission start denied! No valid mission");
1043  }
1044  }
1045  break;
1046 
1047  case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
1048  // if no high latency telemetry exists send a failed acknowledge
1050  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
1051  mavlink_log_critical(&mavlink_log_pub, "Control high latency failed! Telemetry unavailable");
1052  }
1053  }
1054  break;
1055 
1056  case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
1057 
1058  // Switch to orbit state and let the orbit task handle the command further
1059  if (TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags,
1060  &internal_state)) {
1061  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1062 
1063  } else {
1064  cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
1065  }
1066 
1067  break;
1068 
1069  case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
1070  cmd_result = handle_command_motor_test(cmd);
1071  break;
1072 
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:
1104  /* ignore commands that are handled by other parts of the system */
1105  break;
1106 
1107  default:
1108  /* Warn about unsupported commands, this makes sense because only commands
1109  * to this component ID (or all) are passed by mavlink. */
1110  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
1111  break;
1112  }
1113 
1114  if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
1115  /* already warned about unsupported commands in "default" case */
1116  answer_command(cmd, cmd_result, command_ack_pub);
1117  }
1118 
1119  return true;
1120 }
1121 
1122 unsigned
1124 {
1125  if (armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
1126  return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1127  }
1128 
1129  if (_param_com_mot_test_en.get() != 1) {
1130  return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
1131  }
1132 
1133  test_motor_s test_motor{};
1134  test_motor.timestamp = hrt_absolute_time();
1135  test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
1136  int throttle_type = (int)(cmd.param2 + 0.5f);
1137 
1138  if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
1139  return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
1140  }
1141 
1142  int motor_count = (int)(cmd.param5 + 0.5);
1143 
1144  if (motor_count > 1) {
1145  return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
1146  }
1147 
1148  test_motor.action = test_motor_s::ACTION_RUN;
1149  test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f);
1150 
1151  if (test_motor.value < FLT_EPSILON) {
1152  // the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects
1153  test_motor.value = -1.f;
1154  }
1155 
1156  test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f);
1157 
1158  // enforce a timeout and a maximum limit
1159  if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
1160  test_motor.timeout_ms = 3000;
1161  }
1162 
1163  test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
1164  _test_motor_pub.publish(test_motor);
1165 
1166  return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
1167 }
1168 
1169 /**
1170 * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each
1171 * time the vehicle is armed with a good GPS fix.
1172 **/
1173 bool
1175 {
1176  // Need global and local position fix to be able to set home
1177  if (status_flags.condition_global_position_valid && status_flags.condition_local_position_valid) {
1178 
1180 
1181  // Ensure that the GPS accuracy is good enough for intializing home
1182  if ((gpos.eph <= _param_com_home_h_t.get()) && (gpos.epv <= _param_com_home_v_t.get())) {
1183 
1185 
1186  // Set home position
1187  home_position_s home{};
1188 
1189  home.timestamp = hrt_absolute_time();
1190 
1191  home.lat = gpos.lat;
1192  home.lon = gpos.lon;
1193  home.valid_hpos = true;
1194 
1195  home.alt = gpos.alt;
1196  home.valid_alt = true;
1197 
1198  home.x = lpos.x;
1199  home.y = lpos.y;
1200  home.z = lpos.z;
1201 
1202  home.yaw = lpos.yaw;
1203 
1204  home.manual_home = false;
1205 
1206  // play tune first time we initialize HOME
1207  if (!status_flags.condition_home_position_valid) {
1208  tune_home_set(true);
1209  }
1210 
1211  // mark home position as set
1212  status_flags.condition_home_position_valid = _home_pub.update(home);
1213 
1214  return status_flags.condition_home_position_valid;
1215  }
1216  }
1217 
1218  return false;
1219 }
1220 
1221 bool
1223 {
1225 
1226  if (!_home_pub.get().valid_alt && lpos.z_global) {
1227  // handle special case where we are setting only altitude using local position reference
1228  home_position_s home{};
1229  home.alt = lpos.ref_alt;
1230  home.valid_alt = true;
1231 
1232  home.timestamp = hrt_absolute_time();
1233 
1234  return _home_pub.update(home);
1235  }
1236 
1237  return false;
1238 }
1239 
1240 void
1242 {
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;
1248 
1249  // XXX for now just set sensors as initialized
1250  status_flags.condition_system_sensors_initialized = true;
1251 
1252  /* set parameters */
1253  param_t _param_sys_type = param_find("MAV_TYPE");
1254  param_t _param_system_id = param_find("MAV_SYS_ID");
1255  param_t _param_component_id = param_find("MAV_COMP_ID");
1256  param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
1257  param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
1258  param_t _param_ef_time_thres = param_find("COM_EF_TIME");
1259  param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
1260  param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST");
1261  param_t _param_min_stick_change = param_find("COM_RC_STICK_OV");
1262  param_t _param_geofence_action = param_find("GF_ACTION");
1263  param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS");
1264  param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
1265  param_t _param_rc_override = param_find("COM_RC_OVERRIDE");
1266  param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ");
1267  param_t _param_arm_auth_required = param_find("COM_ARM_AUTH_REQ");
1268  param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS");
1269  param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID");
1270  param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT");
1271 
1272  param_t _param_fmode_1 = param_find("COM_FLTMODE1");
1273  param_t _param_fmode_2 = param_find("COM_FLTMODE2");
1274  param_t _param_fmode_3 = param_find("COM_FLTMODE3");
1275  param_t _param_fmode_4 = param_find("COM_FLTMODE4");
1276  param_t _param_fmode_5 = param_find("COM_FLTMODE5");
1277  param_t _param_fmode_6 = param_find("COM_FLTMODE6");
1278 
1279  param_t _param_airmode = param_find("MC_AIRMODE");
1280  param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW");
1281 
1282  status_flags.avoidance_system_required = _param_com_obs_avoid.get();
1283 
1284  /* pthread for slow low prio thread */
1285  pthread_t commander_low_prio_thread;
1286 
1287  /* initialize */
1288  if (led_init() != OK) {
1289  PX4_WARN("LED init failed");
1290  }
1291 
1292  if (buzzer_init() != OK) {
1293  PX4_WARN("Buzzer init failed");
1294  }
1295 
1296  uORB::Subscription power_button_state_sub{ORB_ID(power_button_state)};
1297  {
1298  // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
1299  // in IRQ context.
1300  power_button_state_s button_state{};
1301  button_state.event = 0xff;
1302  power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
1303 
1304  power_button_state_sub.copy(&button_state);
1305  }
1306 
1307  if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
1308  PX4_ERR("Failed to register power notification callback");
1309  }
1310 
1311 
1313 
1314  /* armed topic */
1315  hrt_abstime last_disarmed_timestamp = 0;
1316 
1317  /* command ack */
1318  uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
1319 
1320  /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
1321  mission_init();
1322 
1323  /* Start monitoring loop */
1324  unsigned counter = 0;
1325  int stick_off_counter = 0;
1326  int stick_on_counter = 0;
1327 
1328  bool status_changed = true;
1329  bool param_init_forced = true;
1330 
1332  uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
1333  uORB::Subscription cpuload_sub{ORB_ID(cpuload)};
1334  uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)};
1335  uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)};
1336  uORB::Subscription safety_sub{ORB_ID(safety)};
1337  uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)};
1338  uORB::Subscription subsys_sub{ORB_ID(subsystem_info)};
1339  uORB::Subscription system_power_sub{ORB_ID(system_power)};
1340  uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
1341  uORB::Subscription esc_status_sub{ORB_ID(esc_status)};
1342 
1343  geofence_result_s geofence_result {};
1344 
1345  land_detector.landed = true;
1346 
1347  vtol_status.vtol_in_rw_mode = true; // default for vtol is rotary wing
1348 
1349  control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
1350 
1351  thread_running = true;
1352 
1353  /* update vehicle status to find out vehicle type (required for preflight checks) */
1354  int32_t system_type;
1355  param_get(_param_sys_type, &system_type); // get system type
1356  status.system_type = (uint8_t)system_type;
1357 
1358  if (is_rotary_wing(&status) || is_vtol(&status)) {
1359  status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
1360 
1361  } else if (is_fixed_wing(&status)) {
1363 
1364  } else if (is_ground_rover(&status)) {
1365  status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
1366 
1367  } else {
1368  status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
1369  }
1370 
1371  status.is_vtol = is_vtol(&status);
1372  status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
1373 
1375 
1376  // initially set to failed
1380 
1381  // Run preflight check
1382  int32_t rc_in_off = 0;
1383 
1384  param_get(_param_rc_in_off, &rc_in_off);
1385 
1386  int32_t arm_switch_is_button = 0;
1387  param_get(_param_arm_switch_is_button, &arm_switch_is_button);
1388 
1389  int32_t arm_without_gps_param = 0;
1390  param_get(_param_arm_without_gps, &arm_without_gps_param);
1392 
1393  int32_t arm_mission_required_param = 0;
1394  param_get(_param_arm_mission_required, &arm_mission_required_param);
1395  arm_requirements |= (arm_mission_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1397 
1398  int32_t arm_auth_required_param = 0;
1399  param_get(_param_arm_auth_required, &arm_auth_required_param);
1400  arm_requirements |= (arm_auth_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1402 
1403  int32_t arm_escs_checks_required_param = 0;
1404  param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
1405  arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1407 
1408  status.rc_input_mode = rc_in_off;
1409 
1410  // user adjustable duration required to assert arm/disarm via throttle/rudder stick
1411  int32_t rc_arm_hyst = 100;
1412  param_get(_param_rc_arm_hyst, &rc_arm_hyst);
1413  rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
1414 
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;
1419 
1420  /* RC override auto modes */
1421  int32_t rc_override = 0;
1422 
1423  int32_t takeoff_complete_act = 0;
1424 
1425  /* Thresholds for engine failure detection */
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; /**< absolute time when engine was healty */
1430 
1431  /* check which state machines for changes, clear "changed" flag */
1432  bool failsafe_old = false;
1433 
1434  bool have_taken_off_since_arming = false;
1435 
1436  /* initialize low priority thread */
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));
1440 
1441 #ifndef __PX4_QURT
1442  // This is not supported by QURT (yet).
1443  struct sched_param param;
1444  (void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
1445 
1446  /* low priority */
1447  param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
1448  (void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
1449 #endif
1450 
1451  pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);
1452  pthread_attr_destroy(&commander_low_prio_attr);
1453 
1455 
1456  // run preflight immediately to find all relevant parameters, but don't report
1457  preflight_check(false);
1458 
1459  while (!should_exit()) {
1460 
1462 
1463  /* update parameters */
1464  bool params_updated = _parameter_update_sub.updated();
1465 
1466  if (params_updated || param_init_forced) {
1467  // clear update
1468  parameter_update_s update;
1469  _parameter_update_sub.copy(&update);
1470 
1471  // update parameters from storage
1472  updateParams();
1473 
1474  /* update parameters */
1475  if (!armed.armed) {
1476  if (param_get(_param_sys_type, &system_type) != OK) {
1477  PX4_ERR("failed getting new system type");
1478 
1479  } else {
1480  status.system_type = (uint8_t)system_type;
1481  }
1482 
1483  const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode);
1484  const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !vtol_status.vtol_in_rw_mode);
1485  const bool is_ground = is_ground_rover(&status);
1486 
1487  /* disable manual override for all systems that rely on electronic stabilization */
1488  if (is_rotary) {
1489  status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
1490 
1491  } else if (is_fixed) {
1493 
1494  } else if (is_ground) {
1495  status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
1496  }
1497 
1498  /* set vehicle_status.is_vtol flag */
1499  status.is_vtol = is_vtol(&status);
1500  status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
1501 
1502  /* check and update system / component ID */
1503  int32_t sys_id = 0;
1504  param_get(_param_system_id, &sys_id);
1505  status.system_id = sys_id;
1506 
1507  int32_t comp_id = 0;
1508  param_get(_param_component_id, &comp_id);
1509  status.component_id = comp_id;
1510 
1512 
1513  status_changed = true;
1514  }
1515 
1516  /* Safety parameters */
1517  param_get(_param_rc_in_off, &rc_in_off);
1518  status.rc_input_mode = rc_in_off;
1519  param_get(_param_rc_arm_hyst, &rc_arm_hyst);
1520  param_get(_param_min_stick_change, &min_stick_change);
1521  param_get(_param_rc_override, &rc_override);
1522  // percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
1523  min_stick_change *= 0.02f;
1524  rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
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);
1530 
1531  param_get(_param_arm_switch_is_button, &arm_switch_is_button);
1532 
1533  param_get(_param_arm_without_gps, &arm_without_gps_param);
1535 
1536  param_get(_param_arm_mission_required, &arm_mission_required_param);
1537  arm_requirements |= (arm_mission_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1539 
1540  param_get(_param_arm_auth_required, &arm_auth_required_param);
1541  arm_requirements |= (arm_auth_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1543 
1544  param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
1545  arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE :
1547 
1548 
1549  /* flight mode slots */
1550  param_get(_param_fmode_1, &_flight_mode_slots[0]);
1551  param_get(_param_fmode_2, &_flight_mode_slots[1]);
1552  param_get(_param_fmode_3, &_flight_mode_slots[2]);
1553  param_get(_param_fmode_4, &_flight_mode_slots[3]);
1554  param_get(_param_fmode_5, &_flight_mode_slots[4]);
1555  param_get(_param_fmode_6, &_flight_mode_slots[5]);
1556 
1557  param_get(_param_takeoff_finished_action, &takeoff_complete_act);
1558 
1559  _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
1560 
1561  /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */
1562  if (_param_airmode != PARAM_INVALID && _param_rc_map_arm_switch != PARAM_INVALID) {
1563  param_get(_param_airmode, &airmode);
1564  param_get(_param_rc_map_arm_switch, &rc_map_arm_switch);
1565 
1566  if (airmode == 2 && rc_map_arm_switch == 0) {
1567  airmode = 1; // change to roll/pitch airmode
1568  param_set(_param_airmode, &airmode);
1569  mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch")
1570  }
1571  }
1572 
1573  param_init_forced = false;
1574  }
1575 
1576  /* Update OA parameter */
1577  status_flags.avoidance_system_required = _param_com_obs_avoid.get();
1578 
1579  /* handle power button state */
1580  if (power_button_state_sub.updated()) {
1581  power_button_state_s button_state;
1582  power_button_state_sub.copy(&button_state);
1583 
1584  if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
1585  px4_shutdown_request(false, false);
1586  }
1587  }
1588 
1589  sp_man_sub.update(&sp_man);
1590 
1591  offboard_control_update(status_changed);
1592 
1593  if (system_power_sub.updated()) {
1594  system_power_s system_power = {};
1595  system_power_sub.copy(&system_power);
1596 
1597  if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
1598  if (system_power.servo_valid &&
1599  !system_power.brick_valid &&
1600  !system_power.usb_connected) {
1601  /* flying only on servo rail, this is unsafe */
1602  status_flags.condition_power_input_valid = false;
1603 
1604  } else {
1605  status_flags.condition_power_input_valid = true;
1606  }
1607 
1608  /* if the USB hardware connection went away, reboot */
1609  if (status_flags.usb_connected && !system_power.usb_connected && shutdown_if_allowed()) {
1610  /*
1611  * apparently the USB cable went away but we are still powered,
1612  * so lets reset to a classic non-usb state.
1613  */
1614  mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting.")
1615  px4_usleep(400000);
1616  px4_shutdown_request(true, false);
1617  }
1618  }
1619  }
1620 
1621  /* update safety topic */
1622  if (safety_sub.updated()) {
1623  bool previous_safety_off = safety.safety_off;
1624 
1625  if (safety_sub.copy(&safety)) {
1626 
1627  /* disarm if safety is now on and still armed */
1628  if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF)
1629  && safety.safety_switch_available && !safety.safety_off) {
1630 
1631  if (TRANSITION_CHANGED == arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY,
1632  &armed, true /* fRunPreArmChecks */, &mavlink_log_pub,
1634  ) {
1635  status_changed = true;
1636  }
1637  }
1638 
1639  // Notify the user if the status of the safety switch changes
1640  if (safety.safety_switch_available && previous_safety_off != safety.safety_off) {
1641 
1642  if (safety.safety_off) {
1644 
1645  } else {
1646  tune_neutral(true);
1647  }
1648 
1649  status_changed = true;
1650  }
1651  }
1652  }
1653 
1654  /* update vtol vehicle status*/
1655  if (vtol_vehicle_status_sub.updated()) {
1656  /* vtol status changed */
1657  vtol_vehicle_status_sub.copy(&vtol_status);
1658  status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
1659 
1660  /* Make sure that this is only adjusted if vehicle really is of type vtol */
1661  if (is_vtol(&status)) {
1662 
1663  // Check if there has been any change while updating the flags
1664  const auto new_vehicle_type = vtol_status.vtol_in_rw_mode ?
1665  vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
1667 
1668  if (new_vehicle_type != status.vehicle_type) {
1669  status.vehicle_type = vtol_status.vtol_in_rw_mode ?
1670  vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
1672  status_changed = true;
1673  }
1674 
1675  if (status.in_transition_mode != vtol_status.vtol_in_trans_mode) {
1676  status.in_transition_mode = vtol_status.vtol_in_trans_mode;
1677  status_changed = true;
1678  }
1679 
1680  if (status.in_transition_to_fw != vtol_status.in_transition_to_fw) {
1681  status.in_transition_to_fw = vtol_status.in_transition_to_fw;
1682  status_changed = true;
1683  }
1684 
1685  if (status_flags.vtol_transition_failure != vtol_status.vtol_transition_failsafe) {
1686  status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
1687  status_changed = true;
1688  }
1689 
1690  const bool should_soft_stop = (status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
1691 
1692  if (armed.soft_stop != should_soft_stop) {
1693  armed.soft_stop = should_soft_stop;
1694  status_changed = true;
1695  }
1696  }
1697  }
1698 
1699  if (esc_status_sub.updated()) {
1700  /* ESCs status changed */
1701  esc_status_s esc_status = {};
1702 
1703  esc_status_sub.copy(&esc_status);
1704  esc_status_check(esc_status);
1705  }
1706 
1707  estimator_check(&status_changed);
1708 
1709  /* Update land detector */
1710  if (land_detector_sub.updated()) {
1711  land_detector_sub.copy(&land_detector);
1712 
1713  // Only take actions if armed
1714  if (armed.armed) {
1715  if (was_landed != land_detector.landed) {
1716  if (land_detector.landed) {
1717  mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected");
1718 
1719  } else {
1720  mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected");
1721  have_taken_off_since_arming = true;
1722 
1723  // Set all position and velocity test probation durations to takeoff value
1724  // This is a larger value to give the vehicle time to complete a failsafe landing
1725  // if faulty sensors cause loss of navigation shortly after takeoff.
1726  _gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
1727  _lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
1728  _lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
1729  }
1730  }
1731 
1732  if (was_falling != land_detector.freefall) {
1733  if (land_detector.freefall) {
1734  mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected");
1735  }
1736  }
1737  }
1738 
1739  was_landed = land_detector.landed;
1740  was_falling = land_detector.freefall;
1741  }
1742 
1743 
1744  // Auto disarm when landed or kill switch engaged
1745  if (armed.armed) {
1746 
1747  // Check for auto-disarm on landing or pre-flight
1748  if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
1749 
1750  if (_param_com_disarm_land.get() > 0 && have_taken_off_since_arming) {
1751  _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
1753 
1754  } else if (_param_com_disarm_preflight.get() > 0 && !have_taken_off_since_arming) {
1755  _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
1757  }
1758 
1760  arm_disarm(false, true, &mavlink_log_pub, "Auto disarm initiated");
1761  }
1762  }
1763 
1764  // Auto disarm after 5 seconds if kill switch is engaged
1766 
1768  arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming");
1769  }
1770 
1771  } else {
1774  }
1775 
1776 
1778  // store the last good main_state when not in an navigation
1779  // hold state
1780  main_state_before_rtl = internal_state.main_state;
1781 
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) {
1785  // reset flag again when we switched out of it
1787  }
1788 
1789  cpuload_sub.update(&cpuload);
1790 
1792 
1793  /* update subsystem info which arrives from outside of commander*/
1795 
1796  while (subsys_sub.update(&info)) {
1797  set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
1798  status_changed = true;
1799  }
1800 
1801  /* If in INIT state, try to proceed to STANDBY state */
1802  if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
1803 
1804  arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
1805  true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
1807 
1808  if (arming_ret == TRANSITION_DENIED) {
1809  /* do not complain if not allowed into standby */
1810  arming_ret = TRANSITION_NOT_CHANGED;
1811  }
1812  }
1813 
1814  /* start mission result check */
1815  const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
1816 
1817  if (_mission_result_sub.update()) {
1818  const mission_result_s &mission_result = _mission_result_sub.get();
1819 
1820  // if mission_result is valid for the current mission
1821  const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp)
1822  && (mission_result.instance_count > 0);
1823 
1824  status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
1825 
1826  if (mission_result_ok) {
1827 
1828  if (status.mission_failure != mission_result.failure) {
1829  status.mission_failure = mission_result.failure;
1830  status_changed = true;
1831 
1832  if (status.mission_failure) {
1833  mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed");
1834  }
1835  }
1836 
1837  /* Only evaluate mission state if home is set */
1838  if (status_flags.condition_home_position_valid &&
1839  (prev_mission_instance_count != mission_result.instance_count)) {
1840 
1841  if (!status_flags.condition_auto_mission_available) {
1842  /* the mission is invalid */
1843  tune_mission_fail(true);
1844 
1845  } else if (mission_result.warning) {
1846  /* the mission has a warning */
1847  tune_mission_fail(true);
1848 
1849  } else {
1850  /* the mission is valid */
1851  tune_mission_ok(true);
1852  }
1853  }
1854  }
1855  }
1856 
1857  /* start geofence result check */
1858  geofence_result_sub.update(&geofence_result);
1859 
1860  // Geofence actions
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;
1863 
1864  if (armed.armed
1865  && geofence_action_enabled
1866  && not_in_battery_failsafe) {
1867 
1868  // check for geofence violation transition
1869  if (geofence_result.geofence_violated && !_geofence_violated_prev) {
1870 
1871  switch (geofence_result.geofence_action) {
1872  case (geofence_result_s::GF_ACTION_NONE) : {
1873  // do nothing
1874  break;
1875  }
1876 
1877  case (geofence_result_s::GF_ACTION_WARN) : {
1878  // do nothing, mavlink critical messages are sent by navigator
1879  break;
1880  }
1881 
1882  case (geofence_result_s::GF_ACTION_LOITER) : {
1883  if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
1884  &internal_state)) {
1885  _geofence_loiter_on = true;
1886  }
1887 
1888  break;
1889  }
1890 
1891  case (geofence_result_s::GF_ACTION_RTL) : {
1892  if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
1893  &internal_state)) {
1894  _geofence_rtl_on = true;
1895  }
1896 
1897  break;
1898  }
1899 
1900  case (geofence_result_s::GF_ACTION_TERMINATE) : {
1901  warnx("Flight termination because of geofence");
1902  mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
1903  armed.force_failsafe = true;
1904  status_changed = true;
1905  break;
1906  }
1907  }
1908  }
1909 
1910  _geofence_violated_prev = geofence_result.geofence_violated;
1911 
1912  // reset if no longer in LOITER or if manually switched to LOITER
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;
1915 
1916  if (!in_loiter_mode || manual_loiter_switch_on) {
1917  _geofence_loiter_on = false;
1918  }
1919 
1920 
1921  // reset if no longer in RTL or if manually switched to RTL
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;
1924 
1925  if (!in_rtl_mode || manual_return_switch_on) {
1926  _geofence_rtl_on = false;
1927  }
1928 
1930 
1931  } else {
1932  // No geofence checks, reset flags
1933  _geofence_loiter_on = false;
1934  _geofence_rtl_on = false;
1936  _geofence_violated_prev = false;
1937  }
1938 
1939  // abort auto mode or geofence reaction if sticks are moved significantly
1940  // but only if not in a low battery handling action
1941  const bool not_in_low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
1942  const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
1943  const bool manual_mode_before_geofence =
1944  main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL ||
1945  main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL ||
1946  main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL ||
1947  main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO ||
1948  main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE ||
1949  main_state_before_rtl == commander_state_s::MAIN_STATE_STAB;
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;
1955 
1956  if (rc_override != 0 && is_rotary_wing && not_in_low_battery_reaction
1957  && (in_auto_mode || (_geofence_warning_action_on && manual_mode_before_geofence))) {
1958  // transition to previous state if sticks are touched
1959  if ((_last_sp_man.timestamp != sp_man.timestamp) &&
1960  ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) ||
1961  (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) ||
1962  (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) ||
1963  (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
1964 
1965  // revert to position control in any case
1966  main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
1967  mavlink_log_critical(&mavlink_log_pub, "Autonomy off! Returned control to pilot");
1968  }
1969  }
1970 
1971  /* Check for mission flight termination */
1974 
1975  armed.force_failsafe = true;
1976  status_changed = true;
1977  static bool flight_termination_printed = false;
1978 
1979  if (!flight_termination_printed) {
1980  mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
1981  flight_termination_printed = true;
1982  }
1983 
1984  if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
1985  mavlink_log_critical(&mavlink_log_pub, "Flight termination active");
1986  }
1987  }
1988 
1989  /* RC input check */
1990  if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
1991  (hrt_elapsed_time(&sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
1992 
1993  /* handle the case where RC signal was regained */
1994  if (!status_flags.rc_signal_found_once) {
1995  status_flags.rc_signal_found_once = true;
1996  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
1997  status_changed = true;
1998 
1999  } else {
2000  if (status.rc_signal_lost) {
2001  mavlink_log_info(&mavlink_log_pub, "Manual control regained after %llums",
2003  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
2004  status_changed = true;
2005  }
2006  }
2007 
2008  status.rc_signal_lost = false;
2009 
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;
2014 
2015  /* DISARM
2016  * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
2017  * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
2018  * do it only for rotary wings in manual mode or fixed wing if landed.
2019  * Disable stick-disarming if arming switch or button is mapped */
2020  const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f
2021  && !arm_switch_or_button_mapped;
2022  const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 &&
2023  _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
2024  sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF;
2025 
2026  if (in_armed_state &&
2027  status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
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)) {
2030 
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;
2037 
2038  if (rc_wants_disarm && (land_detector.landed || manual_thrust_mode)) {
2039  arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
2040  true /* fRunPreArmChecks */,
2042  }
2043 
2044  stick_off_counter++;
2045 
2046  } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
2047  /* do not reset the counter when holding the arm button longer than needed */
2048  stick_off_counter = 0;
2049  }
2050 
2051  /* ARM
2052  * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
2053  * and we're in MANUAL mode.
2054  * Disable stick-arming if arming switch or button is mapped */
2055  const bool stick_in_lower_right = sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f
2056  && !arm_switch_or_button_mapped;
2057  /* allow a grace period for re-arming: preflight checks don't need to pass during that time,
2058  * for example for accidential in-air disarming */
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 &&
2061  _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF &&
2062  sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
2063  (sp_man.z < 0.1f || in_arming_grace_period);
2064 
2065  if (!in_armed_state &&
2066  status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
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) {
2069 
2070  /* we check outside of the transition function here because the requirement
2071  * for being in manual mode only applies to manual arming actions.
2072  * the system can be armed in auto if armed via the GCS.
2073  */
2074 
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)
2081  ) {
2082  print_reject_arm("Not arming: Switch to a manual mode first");
2083 
2084  } else if (!status_flags.condition_home_position_valid &&
2085  geofence_action == geofence_result_s::GF_ACTION_RTL) {
2086  print_reject_arm("Not arming: Geofence RTL requires valid home");
2087 
2088  } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
2089  arming_ret = arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
2090  !in_arming_grace_period /* fRunPreArmChecks */,
2092 
2093  if (arming_ret != TRANSITION_CHANGED) {
2094  px4_usleep(100000);
2095  print_reject_arm("Not arming: Preflight checks failed");
2096  }
2097  }
2098  }
2099 
2100  stick_on_counter++;
2101 
2102  } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
2103  /* do not reset the counter when holding the arm button longer than needed */
2104  stick_on_counter = 0;
2105  }
2106 
2108 
2109  if (arming_ret == TRANSITION_DENIED) {
2110  /*
2111  * the arming transition can be denied to a number of reasons:
2112  * - pre-flight check failed (sensors not ok or not calibrated)
2113  * - safety not disabled
2114  * - system not in manual mode
2115  */
2116  tune_negative(true);
2117  }
2118 
2119  /* evaluate the main state machine according to mode switches */
2120  bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);
2121  transition_result_t main_res = set_main_state(status, &status_changed);
2122 
2123  /* store last position lock state */
2127 
2128  /* play tune on mode change only if armed, blink LED always */
2129  if (main_res == TRANSITION_CHANGED || first_rc_eval) {
2130  tune_positive(armed.armed);
2131  status_changed = true;
2132 
2133  } else if (main_res == TRANSITION_DENIED) {
2134  /* DENIED here indicates bug in the commander */
2135  mavlink_log_critical(&mavlink_log_pub, "Switching to this mode is currently not possible");
2136  }
2137 
2138  /* check throttle kill switch */
2139  if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2140  /* set lockdown flag */
2141  if (!armed.manual_lockdown) {
2142  mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged");
2143  status_changed = true;
2144  armed.manual_lockdown = true;
2145  }
2146 
2147  } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
2148  if (armed.manual_lockdown) {
2149  mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged");
2150  status_changed = true;
2151  armed.manual_lockdown = false;
2152  }
2153  }
2154 
2155  /* no else case: do not change lockdown flag in unconfigured case */
2156 
2157  } else {
2158  if (!status_flags.rc_input_blocked && !status.rc_signal_lost) {
2159  mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
2160  status.rc_signal_lost = true;
2162  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
2163  status_changed = true;
2164  }
2165  }
2166 
2167  // data link checks which update the status
2168  data_link_check(status_changed);
2169 
2170  // engine failure detection
2171  // TODO: move out of commander
2172  if (actuator_controls_sub.updated()) {
2173  /* Check engine failure
2174  * only for fixed wing for now
2175  */
2176  if (!status_flags.circuit_breaker_engaged_enginefailure_check &&
2177  status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol && armed.armed) {
2178 
2180  actuator_controls_sub.copy(&actuator_controls);
2181 
2182  const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
2183  const float current2throttle = _battery_current / throttle;
2184 
2185  if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres))
2186  || status.engine_failure) {
2187 
2188  const float elapsed = hrt_elapsed_time(&timestamp_engine_healthy) / 1e6f;
2189 
2190  /* potential failure, measure time */
2191  if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres)
2192  && !status.engine_failure) {
2193 
2194  status.engine_failure = true;
2195  status_changed = true;
2196 
2197  PX4_ERR("Engine Failure");
2198  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, status);
2199  }
2200  }
2201 
2202  } else {
2203  /* no failure reset flag */
2204  timestamp_engine_healthy = hrt_absolute_time();
2205 
2206  if (status.engine_failure) {
2207  status.engine_failure = false;
2208  status_changed = true;
2209  }
2210  }
2211  }
2212 
2213  /* Reset main state to loiter or auto-mission after takeoff is completed.
2214  * Sometimes, the mission result topic is outdated and the mission is still signaled
2215  * as finished even though we only just started with the takeoff. Therefore, we also
2216  * check the timestamp of the mission_result topic. */
2217  if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
2218  && (_mission_result_sub.get().timestamp > internal_state.timestamp)
2220 
2221  const bool mission_available = (_mission_result_sub.get().timestamp > commander_boot_timestamp)
2223 
2224  if ((takeoff_complete_act == 1) && mission_available) {
2225  main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state);
2226 
2227  } else {
2228  main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2229  }
2230  }
2231 
2232  /* check if we are disarmed and there is a better mode to wait in */
2233  if (!armed.armed) {
2234 
2235  /* if there is no radio control but GPS lock the user might want to fly using
2236  * just a tablet. Since the RC will force its mode switch setting on connecting
2237  * we can as well just wait in a hold mode which enables tablet control.
2238  */
2239  if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
2240  && status_flags.condition_home_position_valid) {
2241 
2242  main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2243  }
2244  }
2245 
2246  /* handle commands last, as the system needs to be updated to handle them */
2247  if (cmd_sub.updated()) {
2248  vehicle_command_s cmd{};
2249 
2250  /* got command */
2251  cmd_sub.copy(&cmd);
2252 
2253  /* handle it */
2254  if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) {
2255  status_changed = true;
2256  }
2257  }
2258 
2259  /* Check for failure detector status */
2260  const bool failure_detector_updated = _failure_detector.update(status);
2261 
2262  if (failure_detector_updated) {
2263 
2264  const uint8_t failure_status = _failure_detector.getStatus();
2265 
2266  if (failure_status != status.failure_detector_status) {
2267  status.failure_detector_status = failure_status;
2268  status_changed = true;
2269  }
2270  }
2271 
2272  if (armed.armed &&
2273  failure_detector_updated &&
2276 
2277  if (_failure_detector.isFailure()) {
2278 
2279  armed.force_failsafe = true;
2280  status_changed = true;
2281 
2283 
2284  mavlink_log_critical(&mavlink_log_pub, "Critical failure detected: terminate flight");
2286  }
2287  }
2288 
2289  /* Get current timestamp */
2290  const hrt_abstime now = hrt_absolute_time();
2291 
2292  // automatically set or update home position
2293  if (!_home_pub.get().manual_home) {
2294  const vehicle_local_position_s &local_position = _local_position_sub.get();
2295 
2296  if (armed.armed) {
2297  if ((!was_armed || (was_landed && !land_detector.landed)) &&
2299 
2300  /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
2302  }
2303 
2304  } else {
2305  if (status_flags.condition_home_position_valid) {
2306  if (land_detector.landed && local_position.xy_valid && local_position.z_valid) {
2307  /* distance from home */
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);
2313 
2314  if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) {
2315 
2316  /* update when disarmed, landed and moved away from current home position */
2318  }
2319  }
2320 
2321  } else {
2322  /* First time home position update - but only if disarmed */
2324 
2325  /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
2326  * This allows home altitude to be used in the calculation of height above takeoff location when GPS
2327  * use has commenced after takeoff. */
2328  if (!status_flags.condition_home_position_valid) {
2330  }
2331  }
2332  }
2333  }
2334 
2335  // check for arming state change
2336  if (was_armed != armed.armed) {
2337  status_changed = true;
2338 
2339  if (!armed.armed) { // increase the flight uuid upon disarming
2340  ++flight_uuid;
2341  param_set(_param_flight_uuid, &flight_uuid);
2342  last_disarmed_timestamp = hrt_absolute_time();
2343  }
2344  }
2345 
2346  was_armed = armed.armed;
2347 
2348  /* now set navigation state according to failsafe and main state */
2349  bool nav_state_changed = set_nav_state(&status,
2350  &armed,
2351  &internal_state,
2352  &mavlink_log_pub,
2353  (link_loss_actions_t)_param_nav_dll_act.get(),
2356  status_flags,
2357  land_detector.landed,
2358  (link_loss_actions_t)_param_nav_rcl_act.get(),
2359  (offboard_loss_actions_t)_param_com_obl_act.get(),
2360  (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(),
2361  (position_nav_loss_actions_t)_param_com_posctl_navl.get());
2362 
2363  if (nav_state_changed) {
2365  }
2366 
2367  if (status.failsafe != failsafe_old) {
2368  status_changed = true;
2369 
2370  if (status.failsafe) {
2371  mavlink_log_info(&mavlink_log_pub, "Failsafe mode activated");
2372 
2373  } else {
2374  mavlink_log_info(&mavlink_log_pub, "Failsafe mode deactivated");
2375  }
2376 
2377  failsafe_old = status.failsafe;
2378  }
2379 
2380  /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 1 Hz or immediately when changed */
2381  if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed || nav_state_changed) {
2382 
2384 
2385  status.timestamp = hrt_absolute_time();
2386  _status_pub.publish(status);
2387 
2388  switch ((PrearmedMode)_param_com_prearm_mode.get()) {
2389  case PrearmedMode::DISABLED:
2390  /* skip prearmed state */
2391  armed.prearmed = false;
2392  break;
2393 
2394  case PrearmedMode::ALWAYS:
2395  /* safety is not present, go into prearmed
2396  * (all output drivers should be started / unlocked last in the boot process
2397  * when the rest of the system is fully initialized)
2398  */
2400  break;
2401 
2402  case PrearmedMode::SAFETY_BUTTON:
2403  if (safety.safety_switch_available) {
2404  /* safety switch is present, go into prearmed if safety is off */
2405  armed.prearmed = safety.safety_off;
2406 
2407  } else {
2408  /* safety switch is not present, do not go into prearmed */
2409  armed.prearmed = false;
2410  }
2411 
2412  break;
2413 
2414  default:
2415  armed.prearmed = false;
2416  break;
2417  }
2418 
2419  armed.timestamp = hrt_absolute_time();
2420  _armed_pub.publish(armed);
2421 
2422  /* publish internal state for logging purposes */
2423  internal_state.timestamp = hrt_absolute_time();
2424  _commander_state_pub.publish(internal_state);
2425 
2426  /* publish vehicle_status_flags */
2427  status_flags.timestamp = hrt_absolute_time();
2428  _vehicle_status_flags_pub.publish(status_flags);
2429  }
2430 
2431  /* play arming and battery warning tunes */
2432  if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available
2433  && safety.safety_off))) {
2434  /* play tune when armed */
2436  arm_tune_played = true;
2437 
2438  } else if (!status_flags.usb_connected &&
2439  (status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
2440  (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
2441  /* play tune on battery critical */
2443 
2444  } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
2445  (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
2446  /* play tune on battery warning */
2448 
2449  } else if (status.failsafe) {
2450  tune_failsafe(true);
2451 
2452  } else {
2454  }
2455 
2456  /* reset arm_tune_played when disarmed */
2457  if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
2458 
2459  //Notify the user that it is safe to approach the vehicle
2460  if (arm_tune_played) {
2461  tune_neutral(true);
2462  }
2463 
2464  arm_tune_played = false;
2465  }
2466 
2467  /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
2469 
2470  if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized
2471  && status_flags.condition_system_hotplug_timeout)) {
2472 
2474  sensor_fail_tune_played = true;
2475  status_changed = true;
2476  }
2477 
2478  counter++;
2479 
2480  int blink_state = blink_msg_state();
2481 
2482  if (blink_state > 0) {
2483  /* blinking LED message, don't touch LEDs */
2484  if (blink_state == 2) {
2485  /* blinking LED message completed, restore normal state */
2486  control_status_leds(&status, &armed, true, _battery_warning, &cpuload);
2487  }
2488 
2489  } else {
2490  /* normal state */
2491  control_status_leds(&status, &armed, status_changed, _battery_warning, &cpuload);
2492  }
2493 
2494  status_changed = false;
2495 
2496  if (!armed.armed) {
2497  /* Reset the flag if disarmed. */
2498  have_taken_off_since_arming = false;
2499  }
2500 
2501  arm_auth_update(now, params_updated || param_init_forced);
2502 
2503  px4_usleep(COMMANDER_MONITORING_INTERVAL);
2504  }
2505 
2506  thread_should_exit = true;
2507 
2508  /* wait for threads to complete */
2509  int ret = pthread_join(commander_low_prio_thread, nullptr);
2510 
2511  if (ret) {
2512  warn("join failed: %d", ret);
2513  }
2514 
2515  rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
2516 
2517  /* close fds */
2518  led_deinit();
2519  buzzer_deinit();
2520 
2521  thread_running = false;
2522 }
2523 
2524 void
2526 {
2527  status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
2529  status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
2531  status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(),
2533  status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled_by_val(_param_cbrk_enginefail.get(),
2535  status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_gpsfail.get(),
2537  status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(),
2539  status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled_by_val(_param_cbrk_velposerr.get(),
2541 }
2542 
2543 void
2544 Commander::check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
2545  bool *changed)
2546 {
2548  bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
2549 
2550  if (*valid_out != valid_new) {
2551  *valid_out = valid_new;
2552  *changed = true;
2553  }
2554 }
2555 
2556 void
2557 control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed,
2558  bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local)
2559 {
2560  static hrt_abstime overload_start = 0;
2561 
2562  bool overload = (cpuload_local->load > 0.95f) || (cpuload_local->ram_usage > 0.98f);
2563 
2564  if (overload_start == 0 && overload) {
2565  overload_start = hrt_absolute_time();
2566 
2567  } else if (!overload) {
2568  overload_start = 0;
2569  }
2570 
2571  /* driving rgbled */
2572  if (changed || last_overload != 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;
2576 
2577  uint64_t overload_warn_delay = (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
2578 
2579  /* set mode */
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;
2583 
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;
2587 
2588  } else if (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout) {
2589  led_mode = led_control_s::MODE_BLINK_FAST;
2590  led_color = led_control_s::COLOR_RED;
2591 
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;
2595 
2596  } else if (!status_flags.condition_system_sensors_initialized && !status_flags.condition_system_hotplug_timeout) {
2597  led_mode = led_control_s::MODE_BREATHE;
2598  set_normal_color = true;
2599 
2600  } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_INIT) {
2601  // if in init status it should not be in the error state
2602  led_mode = led_control_s::MODE_OFF;
2603 
2604  } else { // STANDBY_ERROR and other states
2605  led_mode = led_control_s::MODE_BLINK_NORMAL;
2606  led_color = led_control_s::COLOR_RED;
2607  }
2608 
2609  if (set_normal_color) {
2610  /* set color */
2611  if (status.failsafe) {
2612  led_color = led_control_s::COLOR_PURPLE;
2613 
2614  } else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
2615  led_color = led_control_s::COLOR_AMBER;
2616 
2617  } else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
2618  led_color = led_control_s::COLOR_RED;
2619 
2620  } else {
2621  if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) {
2622  led_color = led_control_s::COLOR_GREEN;
2623 
2624  } else {
2625  led_color = led_control_s::COLOR_BLUE;
2626  }
2627  }
2628  }
2629 
2630  if (led_mode != led_control_s::MODE_OFF) {
2631  rgbled_set_color_and_mode(led_color, led_mode);
2632  }
2633  }
2634 
2635  last_overload = overload;
2636 
2637  /* board supports HW armed indicator */
2638 
2639  BOARD_INDICATE_ARMED_STATE(actuator_armed->armed);
2640 
2641 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
2642 
2643  /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
2644  if (actuator_armed->armed) {
2645  if (status.failsafe) {
2646  BOARD_ARMED_LED_OFF();
2647 
2648  if (leds_counter % 5 == 0) {
2649  BOARD_ARMED_STATE_LED_TOGGLE();
2650  }
2651 
2652  } else {
2653  BOARD_ARMED_STATE_LED_OFF();
2654 
2655  /* armed, solid */
2656  BOARD_ARMED_LED_ON();
2657  }
2658 
2659  } else if (actuator_armed->ready_to_arm) {
2660  BOARD_ARMED_LED_OFF();
2661 
2662  /* ready to arm, blink at 1Hz */
2663  if (leds_counter % 20 == 0) {
2664  BOARD_ARMED_STATE_LED_TOGGLE();
2665  }
2666 
2667  } else {
2668  BOARD_ARMED_LED_OFF();
2669 
2670  /* not ready to arm, blink at 10Hz */
2671  if (leds_counter % 2 == 0) {
2672  BOARD_ARMED_STATE_LED_TOGGLE();
2673  }
2674  }
2675 
2676 #endif
2677 
2678  /* give system warnings on error LED */
2679  if (overload) {
2680  if (leds_counter % 2 == 0) {
2681  BOARD_OVERLOAD_LED_TOGGLE();
2682  }
2683 
2684  } else {
2685  BOARD_OVERLOAD_LED_OFF();
2686  }
2687 
2688  leds_counter++;
2689 }
2690 
2692 Commander::set_main_state(const vehicle_status_s &status_local, bool *changed)
2693 {
2694  if (safety.override_available && safety.override_enabled) {
2695  return set_main_state_override_on(status_local, changed);
2696 
2697  } else {
2698  return set_main_state_rc(status_local, changed);
2699  }
2700 }
2701 
2703 Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed)
2704 {
2705  const transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags,
2706  &internal_state);
2707  *changed = (res == TRANSITION_CHANGED);
2708 
2709  return res;
2710 }
2711 
2713 Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed)
2714 {
2715  /* set main state according to RC switches */
2717 
2718  // Note: even if status_flags.offboard_control_set_by_command is set
2719  // we want to allow rc mode change to take precidence. This is a safety
2720  // feature, just in case offboard control goes crazy.
2721 
2722  const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid);
2723  const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid);
2724  const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid);
2725  const bool first_time_rc = (_last_sp_man.timestamp == 0);
2726  const bool rc_values_updated = (_last_sp_man.timestamp != sp_man.timestamp);
2727  const bool some_switch_changed =
2728  (_last_sp_man.offboard_switch != sp_man.offboard_switch)
2729  || (_last_sp_man.return_switch != sp_man.return_switch)
2730  || (_last_sp_man.mode_switch != sp_man.mode_switch)
2731  || (_last_sp_man.acro_switch != sp_man.acro_switch)
2732  || (_last_sp_man.rattitude_switch != sp_man.rattitude_switch)
2733  || (_last_sp_man.posctl_switch != sp_man.posctl_switch)
2734  || (_last_sp_man.loiter_switch != sp_man.loiter_switch)
2735  || (_last_sp_man.mode_slot != sp_man.mode_slot)
2736  || (_last_sp_man.stab_switch != sp_man.stab_switch)
2737  || (_last_sp_man.man_switch != sp_man.man_switch);
2738 
2739  // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
2740  const bool should_evaluate_rc_mode_switch = first_time_rc
2741  || altitude_got_valid
2742  || lpos_got_valid
2743  || gpos_got_valid
2744  || (rc_values_updated && some_switch_changed);
2745 
2746  if (!should_evaluate_rc_mode_switch) {
2747 
2748  // store the last manual control setpoint set by the pilot in a manual state
2749  // if the system now later enters an autonomous state the pilot can move
2750  // the sticks to break out of the autonomous state
2751 
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)) {
2759 
2760  _last_sp_man.timestamp = sp_man.timestamp;
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;
2765  }
2766 
2767  /* no timestamp change or no switch change -> nothing changed */
2768  return TRANSITION_NOT_CHANGED;
2769  }
2770 
2771  _last_sp_man = sp_man;
2772 
2773  // reset the position and velocity validity calculation to give the best change of being able to select
2774  // the desired mode
2775  reset_posvel_validity(changed);
2776 
2777  /* offboard switch overrides main switch */
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);
2780 
2781  if (res == TRANSITION_DENIED) {
2782  print_reject_mode("OFFBOARD");
2783  /* mode rejected, continue to evaluate the main system mode */
2784 
2785  } else {
2786  /* changed successfully or already in this state */
2787  return res;
2788  }
2789  }
2790 
2791  /* RTL switch overrides main switch */
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);
2794 
2795  if (res == TRANSITION_DENIED) {
2796  print_reject_mode("AUTO RTL");
2797 
2798  /* fallback to LOITER if home position not set */
2799  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
2800  }
2801 
2802  if (res != TRANSITION_DENIED) {
2803  /* changed successfully or already in this state */
2804  return res;
2805  }
2806 
2807  /* if we get here mode was rejected, continue to evaluate the main system mode */
2808  }
2809 
2810  /* Loiter switch overrides main switch */
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);
2813 
2814  if (res == TRANSITION_DENIED) {
2815  print_reject_mode("AUTO HOLD");
2816 
2817  } else {
2818  return res;
2819  }
2820  }
2821 
2822  /* we know something has changed - check if we are in mode slot operation */
2823  if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
2824 
2825  if (sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
2826  warnx("m slot overflow");
2827  return TRANSITION_DENIED;
2828  }
2829 
2830  int new_mode = _flight_mode_slots[sp_man.mode_slot - 1];
2831 
2832  if (new_mode < 0) {
2833  /* slot is unused */
2834  res = TRANSITION_NOT_CHANGED;
2835 
2836  } else {
2837  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2838 
2839  /* ensure that the mode selection does not get stuck here */
2840  int maxcount = 5;
2841 
2842  /* enable the use of break */
2843  /* fallback strategies, give the user the closest mode to what he wanted */
2844  while (res == TRANSITION_DENIED && maxcount > 0) {
2845 
2846  maxcount--;
2847 
2848  if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
2849 
2850  /* fall back to loiter */
2851  new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2852  print_reject_mode("AUTO MISSION");
2853  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2854 
2855  if (res != TRANSITION_DENIED) {
2856  break;
2857  }
2858  }
2859 
2860  if (new_mode == commander_state_s::MAIN_STATE_AUTO_RTL) {
2861 
2862  /* fall back to position control */
2863  new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2864  print_reject_mode("AUTO RTL");
2865  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2866 
2867  if (res != TRANSITION_DENIED) {
2868  break;
2869  }
2870  }
2871 
2872  if (new_mode == commander_state_s::MAIN_STATE_AUTO_LAND) {
2873 
2874  /* fall back to position control */
2875  new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2876  print_reject_mode("AUTO LAND");
2877  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2878 
2879  if (res != TRANSITION_DENIED) {
2880  break;
2881  }
2882  }
2883 
2884  if (new_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
2885 
2886  /* fall back to position control */
2887  new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2888  print_reject_mode("AUTO TAKEOFF");
2889  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2890 
2891  if (res != TRANSITION_DENIED) {
2892  break;
2893  }
2894  }
2895 
2896  if (new_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET) {
2897 
2898  /* fall back to position control */
2899  new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
2900  print_reject_mode("AUTO FOLLOW");
2901  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2902 
2903  if (res != TRANSITION_DENIED) {
2904  break;
2905  }
2906  }
2907 
2908  if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
2909 
2910  /* fall back to position control */
2911  new_mode = commander_state_s::MAIN_STATE_POSCTL;
2912  print_reject_mode("AUTO HOLD");
2913  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2914 
2915  if (res != TRANSITION_DENIED) {
2916  break;
2917  }
2918  }
2919 
2920  if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
2921 
2922  /* fall back to altitude control */
2923  new_mode = commander_state_s::MAIN_STATE_ALTCTL;
2924  print_reject_mode("POSITION CONTROL");
2925  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2926 
2927  if (res != TRANSITION_DENIED) {
2928  break;
2929  }
2930  }
2931 
2932  if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
2933 
2934  /* fall back to stabilized */
2935  new_mode = commander_state_s::MAIN_STATE_STAB;
2936  print_reject_mode("ALTITUDE CONTROL");
2937  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2938 
2939  if (res != TRANSITION_DENIED) {
2940  break;
2941  }
2942  }
2943 
2944  if (new_mode == commander_state_s::MAIN_STATE_STAB) {
2945 
2946  /* fall back to manual */
2947  new_mode = commander_state_s::MAIN_STATE_MANUAL;
2948  print_reject_mode("STABILIZED");
2949  res = main_state_transition(status_local, new_mode, status_flags, &internal_state);
2950 
2951  if (res != TRANSITION_DENIED) {
2952  break;
2953  }
2954  }
2955  }
2956  }
2957 
2958  return res;
2959  }
2960 
2961  /* offboard and RTL switches off or denied, check main mode switch */
2962  switch (sp_man.mode_switch) {
2963  case manual_control_setpoint_s::SWITCH_POS_NONE:
2964  res = TRANSITION_NOT_CHANGED;
2965  break;
2966 
2967  case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
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) {
2970  /*
2971  * Legacy mode:
2972  * Acro switch being used as stabilized switch in FW.
2973  */
2974  if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2975  /* manual mode is stabilized already for multirotors, so switch to acro
2976  * for any non-manual mode
2977  */
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);
2980 
2982  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
2983 
2984  } else {
2985  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
2986  }
2987 
2988  } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
2989  /* Similar to acro transitions for multirotors. FW aircraft don't need a
2990  * rattitude mode.*/
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);
2993 
2994  } else {
2995  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
2996  }
2997 
2998  } else {
2999  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3000  }
3001 
3002  } else {
3003  /* New mode:
3004  * - Acro is Acro
3005  * - Manual is not default anymore when the manaul switch is assigned
3006  */
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);
3009 
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);
3012 
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);
3015 
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);
3018 
3019  } else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
3020  // default to MANUAL when no manual switch is set
3021  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3022 
3023  } else {
3024  // default to STAB when the manual switch is assigned (but off)
3025  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
3026  }
3027  }
3028 
3029  // TRANSITION_DENIED is not possible here
3030  break;
3031 
3032  case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
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);
3035 
3036  if (res != TRANSITION_DENIED) {
3037  break; // changed successfully or already in this state
3038  }
3039 
3040  print_reject_mode("POSITION CONTROL");
3041  }
3042 
3043  // fallback to ALTCTL
3044  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
3045 
3046  if (res != TRANSITION_DENIED) {
3047  break; // changed successfully or already in this mode
3048  }
3049 
3050  if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
3051  print_reject_mode("ALTITUDE CONTROL");
3052  }
3053 
3054  // fallback to MANUAL
3055  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3056  // TRANSITION_DENIED is not possible here
3057  break;
3058 
3059  case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
3060  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state);
3061 
3062  if (res != TRANSITION_DENIED) {
3063  break; // changed successfully or already in this state
3064  }
3065 
3066  print_reject_mode("AUTO MISSION");
3067 
3068  // fallback to LOITER if home position not set
3069  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state);
3070 
3071  if (res != TRANSITION_DENIED) {
3072  break; // changed successfully or already in this state
3073  }
3074 
3075  // fallback to POSCTL
3076  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
3077 
3078  if (res != TRANSITION_DENIED) {
3079  break; // changed successfully or already in this state
3080  }
3081 
3082  // fallback to ALTCTL
3083  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, status_flags, &internal_state);
3084 
3085  if (res != TRANSITION_DENIED) {
3086  break; // changed successfully or already in this state
3087  }
3088 
3089  // fallback to MANUAL
3090  res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
3091  // TRANSITION_DENIED is not possible here
3092  break;
3093 
3094  default:
3095  break;
3096  }
3097 
3098  return res;
3099 }
3100 
3101 void
3103 {
3104  // reset all the check probation times back to the minimum value
3108 
3109  const vehicle_local_position_s &local_position = _local_position_sub.get();
3110  const vehicle_global_position_s &global_position = _global_position_sub.get();
3111 
3112  // recheck validity
3113  if (!_skip_pos_accuracy_check) {
3114  check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp,
3116  }
3117 
3118  check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold_adj, local_position.timestamp,
3120  check_posvel_validity(local_position.v_xy_valid, local_position.evh, _param_com_vel_fs_evh.get(),
3121  local_position.timestamp,
3123 }
3124 
3125 bool
3126 Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
3127  const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
3128  bool *validity_changed)
3129 {
3130  const bool was_valid = *valid_state;
3131  bool valid = was_valid;
3132 
3133  // constrain probation times
3134  if (land_detector.landed) {
3135  *probation_time_us = POSVEL_PROBATION_MIN;
3136  }
3137 
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);
3141 
3142  const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
3143 
3144  // Check accuracy with hysteresis in both test level and time
3145  if (level_check_pass) {
3146  if (was_valid) {
3147  // still valid, continue to decrease probation time
3148  const int64_t probation_time_new = *probation_time_us - hrt_elapsed_time(last_fail_time_us);
3149  *probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
3150 
3151  } else {
3152  // check if probation period has elapsed
3153  if (hrt_elapsed_time(last_fail_time_us) > *probation_time_us) {
3154  valid = true;
3155  }
3156  }
3157 
3158  } else {
3159  // level check failed
3160  if (was_valid) {
3161  // FAILURE! no longer valid
3162  valid = false;
3163 
3164  } else {
3165  // failed again, increase probation time
3166  const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) *
3167  _param_com_pos_fs_gain.get();
3168  *probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
3169  }
3170 
3171  *last_fail_time_us = hrt_absolute_time();
3172  }
3173 
3174  if (was_valid != valid) {
3175  *validity_changed = true;
3176  *valid_state = valid;
3177  }
3178 
3179  return valid;
3180 }
3181 
3182 void
3184 {
3185  vehicle_control_mode_s control_mode{};
3186 
3187  control_mode.timestamp = hrt_absolute_time();
3188 
3189  /* set vehicle_control_mode according to set_navigation_state */
3190  control_mode.flag_armed = armed.armed;
3191  control_mode.flag_external_manual_override_ok = (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
3192  && !status.is_vtol);
3193 
3194  switch (status.nav_state) {
3195  case vehicle_status_s::NAVIGATION_STATE_MANUAL:
3196  control_mode.flag_control_manual_enabled = true;
3197  control_mode.flag_control_rates_enabled = stabilization_required();
3198  control_mode.flag_control_attitude_enabled = stabilization_required();
3199  break;
3200 
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;
3205  break;
3206 
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;
3212  break;
3213 
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;
3220  break;
3221 
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;
3228  control_mode.flag_control_position_enabled = !status.in_transition_mode;
3229  control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
3230  break;
3231 
3232  case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
3233  case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
3234  /* override is not ok for the RTL and recovery mode */
3235  control_mode.flag_external_manual_override_ok = false;
3236 
3237  /* fallthrough */
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;
3251  control_mode.flag_control_position_enabled = !status.in_transition_mode;
3252  control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
3253  break;
3254 
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;
3259  break;
3260 
3261  case vehicle_status_s::NAVIGATION_STATE_ACRO:
3262  control_mode.flag_control_manual_enabled = true;
3263  control_mode.flag_control_rates_enabled = true;
3264  break;
3265 
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;
3271  break;
3272 
3273  case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
3274  /* disable all controllers on termination */
3275  control_mode.flag_control_termination_enabled = true;
3276  break;
3277 
3278  case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
3279 
3280  const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get();
3281 
3282  control_mode.flag_control_offboard_enabled = true;
3283 
3284  /*
3285  * The control flags depend on what is ignored according to the offboard control mode topic
3286  * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
3287  */
3288  control_mode.flag_control_rates_enabled =
3289  !offboard_control_mode.ignore_bodyrate_x ||
3290  !offboard_control_mode.ignore_bodyrate_y ||
3291  !offboard_control_mode.ignore_bodyrate_z ||
3292  !offboard_control_mode.ignore_attitude ||
3293  !offboard_control_mode.ignore_position ||
3294  !offboard_control_mode.ignore_velocity ||
3295  !offboard_control_mode.ignore_acceleration_force;
3296 
3297  control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
3298  !offboard_control_mode.ignore_position ||
3299  !offboard_control_mode.ignore_velocity ||
3300  !offboard_control_mode.ignore_acceleration_force;
3301 
3302  // TO-DO: Add support for other modes than yawrate control
3303  control_mode.flag_control_yawrate_override_enabled =
3304  offboard_control_mode.ignore_bodyrate_x &&
3305  offboard_control_mode.ignore_bodyrate_y &&
3306  !offboard_control_mode.ignore_bodyrate_z &&
3307  !offboard_control_mode.ignore_attitude;
3308 
3309  control_mode.flag_control_rattitude_enabled = false;
3310 
3311  control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
3312  !status.in_transition_mode;
3313 
3314  control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
3315  !offboard_control_mode.ignore_position) && !status.in_transition_mode &&
3316  !control_mode.flag_control_acceleration_enabled;
3317 
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;
3320 
3321  control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&
3322  !control_mode.flag_control_acceleration_enabled;
3323 
3324  control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||
3325  !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;
3326 
3327  }
3328  break;
3329 
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;
3338  control_mode.flag_control_position_enabled = !status.in_transition_mode;
3339  control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
3340  control_mode.flag_control_acceleration_enabled = false;
3341  control_mode.flag_control_termination_enabled = false;
3342  break;
3343 
3344  default:
3345  break;
3346  }
3347 
3348  _control_mode_pub.publish(control_mode);
3349 }
3350 
3351 bool
3353 {
3354  return (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || // is a rotary wing, or
3355  status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or
3356  (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND
3357  status.vehicle_type ==
3358  vehicle_status_s::VEHICLE_TYPE_FIXED_WING)); // is a fixed wing, ie: transitioning back to rotary wing mode
3359 }
3360 
3361 void
3363 {
3365 
3368  mavlink_log_critical(&mavlink_log_pub, "REJECT %s", msg);
3369 
3370  /* only buzz if armed, because else we're driving people nuts indoors
3371  they really need to look at the leds as well. */
3372  tune_negative(armed.armed);
3373  }
3374 }
3375 
3376 void
3377 print_reject_arm(const char *msg)
3378 {
3380 
3384  tune_negative(true);
3385  }
3386 }
3387 
3388 void answer_command(const vehicle_command_s &cmd, unsigned result,
3390 {
3391  switch (result) {
3392  case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
3393  break;
3394 
3395  case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
3396  tune_negative(true);
3397  break;
3398 
3399  case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
3400  tune_negative(true);
3401  break;
3402 
3403  case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
3404  tune_negative(true);
3405  break;
3406 
3407  case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
3408  tune_negative(true);
3409  break;
3410 
3411  default:
3412  break;
3413  }
3414 
3415  /* publish ACK */
3416  vehicle_command_ack_s command_ack{};
3417 
3418  command_ack.timestamp = hrt_absolute_time();
3419  command_ack.command = cmd.command;
3420  command_ack.result = (uint8_t)result;
3421  command_ack.target_system = cmd.source_system;
3422  command_ack.target_component = cmd.source_component;
3423 
3424 
3425  command_ack_pub.publish(command_ack);
3426 }
3427 
3428 void *commander_low_prio_loop(void *arg)
3429 {
3430  /* Set thread name */
3431  px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
3432 
3433  /* Subscribe to command topic */
3434  int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
3435 
3436  /* command ack */
3437  uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
3438 
3439  /* wakeup source(s) */
3440  px4_pollfd_struct_t fds[1];
3441 
3442  fds[0].fd = cmd_sub;
3443  fds[0].events = POLLIN;
3444 
3445  while (!thread_should_exit) {
3446  /* wait for up to 1000ms for data */
3447  int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
3448 
3449  if (pret < 0) {
3450  /* this is undesirable but not much we can do - might want to flag unhappy status */
3451  warn("commander: poll error %d, %d", pret, errno);
3452  continue;
3453 
3454  } else if (pret != 0) {
3455  struct vehicle_command_s cmd {};
3456 
3457  /* if we reach here, we have a valid command */
3458  orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
3459 
3460  /* ignore commands the high-prio loop or the navigator handles */
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) {
3466 
3467  continue;
3468  }
3469 
3470  /* only handle low-priority commands here */
3471  switch (cmd.command) {
3472 
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);
3476  /* do nothing for autopilot */
3477  break;
3478  }
3479 
3480  if (is_safe(safety, armed)) {
3481 
3482  if (((int)(cmd.param1)) == 1) {
3483  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3484  px4_usleep(100000);
3485  /* reboot */
3486  px4_shutdown_request(true, false);
3487 
3488  } else if (((int)(cmd.param1)) == 2) {
3489  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3490  px4_usleep(100000);
3491  /* shutdown */
3492  px4_shutdown_request(false, false);
3493 
3494  } else if (((int)(cmd.param1)) == 3) {
3495  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3496  px4_usleep(100000);
3497  /* reboot to bootloader */
3498  px4_shutdown_request(true, true);
3499 
3500  } else {
3501  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3502  }
3503 
3504  } else {
3505  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3506  }
3507 
3508  break;
3509 
3510  case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
3511 
3512  int calib_ret = PX4_ERROR;
3513 
3514  /* try to go to INIT/PREFLIGHT arming state */
3515  if (TRANSITION_DENIED == arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
3516  false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
3518 
3519  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3520  break;
3521 
3522  } else {
3523  status_flags.condition_calibration_enabled = true;
3524  }
3525 
3526  if ((int)(cmd.param1) == 1) {
3527  /* gyro calibration */
3528  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3529  calib_ret = do_gyro_calibration(&mavlink_log_pub);
3530 
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) {
3534  /* temperature calibration: handled in events module */
3535  break;
3536 
3537  } else if ((int)(cmd.param2) == 1) {
3538  /* magnetometer calibration */
3539  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3540  calib_ret = do_mag_calibration(&mavlink_log_pub);
3541 
3542  } else if ((int)(cmd.param3) == 1) {
3543  /* zero-altitude pressure calibration */
3544  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
3545 
3546  } else if ((int)(cmd.param4) == 1) {
3547  /* RC calibration */
3548  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3549  /* disable RC control input completely */
3550  status_flags.rc_input_blocked = true;
3551  calib_ret = OK;
3552  mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
3553 
3554  } else if ((int)(cmd.param4) == 2) {
3555  /* RC trim calibration */
3556  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3557  calib_ret = do_trim_calibration(&mavlink_log_pub);
3558 
3559  } else if ((int)(cmd.param5) == 1) {
3560  /* accelerometer calibration */
3561  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3562  calib_ret = do_accel_calibration(&mavlink_log_pub);
3563 
3564  } else if ((int)(cmd.param5) == 2) {
3565  // board offset calibration
3566  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3567  calib_ret = do_level_calibration(&mavlink_log_pub);
3568 
3569  } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
3570  // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
3571  /* airspeed calibration */
3572  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3574 
3575  } else if ((int)(cmd.param7) == 1) {
3576  /* do esc calibration */
3577  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3578  calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
3579 
3580  } else if ((int)(cmd.param4) == 0) {
3581  /* RC calibration ended - have we been in one worth confirming? */
3582  if (status_flags.rc_input_blocked) {
3583  /* enable RC control input */
3584  status_flags.rc_input_blocked = false;
3585  mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
3586  }
3587 
3588  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3589  /* this always succeeds */
3590  calib_ret = OK;
3591 
3592  } else {
3593  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
3594  }
3595 
3596  status_flags.condition_calibration_enabled = false;
3597 
3598  if (calib_ret == OK) {
3599  tune_positive(true);
3600 
3602 
3603  arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
3604  false /* fRunPreArmChecks */,
3606 
3607  } else {
3608  tune_negative(true);
3609  }
3610 
3611  break;
3612  }
3613 
3614  case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
3615 
3616  if (((int)(cmd.param1)) == 0) {
3617  int ret = param_load_default();
3618 
3619  if (ret == OK) {
3620  mavlink_log_info(&mavlink_log_pub, "Settings loaded");
3621  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3622 
3623  } else {
3624  mavlink_log_critical(&mavlink_log_pub, "Error loading settings");
3625 
3626  /* convenience as many parts of NuttX use negative errno */
3627  if (ret < 0) {
3628  ret = -ret;
3629  }
3630 
3631  if (ret < 1000) {
3632  mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
3633  }
3634 
3635  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
3636  }
3637 
3638  } else if (((int)(cmd.param1)) == 1) {
3639 
3640 #ifdef __PX4_QURT
3641  // TODO FIXME: on snapdragon the save happens too early when the params
3642  // are not set yet. We therefore need to wait some time first.
3643  px4_usleep(1000000);
3644 #endif
3645 
3646  int ret = param_save_default();
3647 
3648  if (ret == OK) {
3649  /* do not spam MAVLink, but provide the answer / green led mechanism */
3650  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3651 
3652  } else {
3653  mavlink_log_critical(&mavlink_log_pub, "Error saving settings");
3654 
3655  /* convenience as many parts of NuttX use negative errno */
3656  if (ret < 0) {
3657  ret = -ret;
3658  }
3659 
3660  if (ret < 1000) {
3661  mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
3662  }
3663 
3664  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
3665  }
3666 
3667  } else if (((int)(cmd.param1)) == 2) {
3668 
3669  /* reset parameters and save empty file */
3670  param_reset_all();
3671 
3672  /* do not spam MAVLink, but provide the answer / green led mechanism */
3673  mavlink_log_critical(&mavlink_log_pub, "Onboard parameters reset");
3674  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3675  }
3676 
3677  break;
3678  }
3679 
3680  case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
3681  /* just ack, implementation handled in the IO driver */
3682  answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
3683  break;
3684 
3685  default:
3686  /* don't answer on unsupported commands, it will be done in main loop */
3687  break;
3688  }
3689  }
3690  }
3691 
3692  px4_close(cmd_sub);
3693 
3694  return nullptr;
3695 }
3696 
3697 int Commander::custom_command(int argc, char *argv[])
3698 {
3699  return print_usage("unknown command");
3700 }
3701 
3702 int Commander::print_usage(const char *reason)
3703 {
3704  if (reason) {
3705  PX4_WARN("%s\n", reason);
3706  }
3707 
3708  return 0;
3709 }
3710 
3711 int Commander::task_spawn(int argc, char *argv[])
3712 {
3713  _task_id = px4_task_spawn_cmd("commander",
3714  SCHED_DEFAULT,
3715  SCHED_PRIORITY_DEFAULT + 40,
3716  3250,
3717  (px4_main_t)&run_trampoline,
3718  (char *const *)argv);
3719 
3720  if (_task_id < 0) {
3721  _task_id = -1;
3722  return -errno;
3723  }
3724 
3725  return 0;
3726 }
3727 
3728 Commander *Commander::instantiate(int argc, char *argv[])
3729 {
3730  Commander *instance = new Commander();
3731 
3732  if (instance) {
3733  if (argc >= 2 && !strcmp(argv[1], "-h")) {
3734  instance->enable_hil();
3735  }
3736  }
3737 
3738  return instance;
3739 }
3740 
3742 {
3743  status.hil_state = vehicle_status_s::HIL_STATE_ON;
3744 }
3745 
3747 {
3748  /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
3749  mission_s mission = {};
3750 
3751  if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
3753  if (mission.count > 0) {
3754  PX4_INFO("Mission #%d loaded, %u WPs, curr: %d", mission.dataman_id, mission.count, mission.current_seq);
3755  }
3756 
3757  } else {
3758  PX4_ERR("reading mission state failed");
3759 
3760  /* initialize mission state in dataman */
3761  mission.timestamp = hrt_absolute_time();
3764  }
3765 
3766  orb_advert_t mission_pub = orb_advertise(ORB_ID(mission), &mission);
3767  orb_unadvertise(mission_pub);
3768  }
3769 }
3770 
3772 {
3773  const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT);
3774 
3775  const bool success = PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false,
3777 
3778  if (success) {
3779  status_flags.condition_system_sensors_initialized = true;
3780  }
3781 
3782  return success;
3783 }
3784 
3785 void Commander::data_link_check(bool &status_changed)
3786 {
3788 
3789  telemetry_status_s telemetry;
3790 
3791  if (_telemetry_status_sub.copy(&telemetry)) {
3792 
3793  // handle different radio types
3794  switch (telemetry.type) {
3795  case telemetry_status_s::LINK_TYPE_USB:
3796  // set (but don't unset) telemetry via USB as active once a MAVLink connection is up
3797  status_flags.usb_connected = true;
3798  break;
3799 
3800  case telemetry_status_s::LINK_TYPE_IRIDIUM: {
3801 
3802  iridiumsbd_status_s iridium_status;
3803 
3804  if (_iridiumsbd_status_sub.update(&iridium_status)) {
3806 
3807  if (status.high_latency_data_link_lost) {
3808  if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
3809  status.high_latency_data_link_lost = false;
3810  status_changed = true;
3811  }
3812  }
3813  }
3814 
3815  break;
3816  }
3817  }
3818 
3819  // handle different remote types
3820  switch (telemetry.remote_type) {
3821  case telemetry_status_s::MAV_TYPE_GCS:
3822 
3823  // Recover from data link lost
3824  if (status.data_link_lost) {
3825  if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
3826  status.data_link_lost = false;
3827  status_changed = true;
3828 
3829  if (_datalink_last_heartbeat_gcs != 0) {
3830  mavlink_log_info(&mavlink_log_pub, "Data link regained");
3831  }
3832  }
3833  }
3834 
3835  // Only keep the very last heartbeat timestamp, so we don't get confused
3836  // by multiple mavlink instances publishing different timestamps.
3837  if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
3839  }
3840 
3841  break;
3842 
3843  case telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER:
3844 
3847  mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
3848  _onboard_controller_lost = false;
3849  status_changed = true;
3850  }
3851 
3852  }
3853 
3855 
3856  if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
3859  }
3860 
3863 
3864  if (_avoidance_system_lost) {
3865  mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
3866  status_changed = true;
3867  _avoidance_system_lost = false;
3868  status_flags.avoidance_system_valid = true;
3869  }
3870  }
3871 
3872  break;
3873  }
3874  }
3875  }
3876 
3877 
3878  // GCS data link loss failsafe
3879  if (!status.data_link_lost) {
3881  && hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
3882 
3883  status.data_link_lost = true;
3884  status.data_link_lost_counter++;
3885 
3886  mavlink_log_critical(&mavlink_log_pub, "Data link lost");
3887 
3888  status_changed = true;
3889  }
3890  }
3891 
3892  // ONBOARD CONTROLLER data link loss failsafe (hard coded 5 seconds)
3896 
3897  mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost");
3898  _onboard_controller_lost = true;
3899  status_changed = true;
3900  }
3901 
3902  // AVOIDANCE SYSTEM state check (only if it is enabled)
3903  if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
3904 
3905  //if avoidance never started
3907  && hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) {
3909  mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available");
3911 
3912  }
3913  }
3914 
3915  //if heartbeats stop
3918  _avoidance_system_lost = true;
3919  mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost");
3920  status_flags.avoidance_system_valid = false;
3921  _print_avoidance_msg_once = false;
3922  }
3923 
3924  //if status changed
3926  if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
3927  mavlink_log_info(&mavlink_log_pub, "Avoidance system starting");
3928  }
3929 
3930  if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
3931  mavlink_log_info(&mavlink_log_pub, "Avoidance system connected");
3932  status_flags.avoidance_system_valid = true;
3933  }
3934 
3935  if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
3936  mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out");
3937  }
3938 
3939  if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
3940  mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected");
3941  status_flags.avoidance_system_valid = false;
3942  status_changed = true;
3943  }
3944 
3946  }
3947  }
3948 
3949 
3950  // high latency data link loss failsafe
3952  && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
3954 
3955  if (!status.high_latency_data_link_lost) {
3956  status.high_latency_data_link_lost = true;
3957  mavlink_log_critical(&mavlink_log_pub, "High latency data link lost");
3958  status_changed = true;
3959  }
3960  }
3961 }
3962 
3964 {
3965  /* update battery status */
3966  if (_battery_sub.updated()) {
3967  battery_status_s battery{};
3968 
3969  if (_battery_sub.copy(&battery)) {
3970 
3971  if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
3972  && battery.connected
3973  && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) {
3974 
3975  status_flags.condition_battery_healthy = true;
3976 
3977  } else {
3978  status_flags.condition_battery_healthy = false;
3979  }
3980 
3981  // execute battery failsafe if the state has gotten worse
3982  if (armed.armed) {
3983  if (battery.warning > _battery_warning) {
3984  battery_failsafe(&mavlink_log_pub, status, status_flags, &internal_state, battery.warning,
3985  (low_battery_action_t)_param_com_low_bat_act.get());
3986  }
3987  }
3988 
3989  // Handle shutdown request from emergency battery action
3990  if (battery.warning != _battery_warning) {
3991 
3992  if ((battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) {
3993  mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down");
3994  px4_usleep(200000);
3995 
3996  int ret_val = px4_shutdown_request(false, false);
3997 
3998  if (ret_val) {
3999  mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown");
4000 
4001  } else {
4002  while (1) { px4_usleep(1); }
4003  }
4004  }
4005  }
4006 
4007  // save last value
4008  _battery_warning = battery.warning;
4009  _battery_current = battery.current_filtered_a;
4010  }
4011  }
4012 }
4013 
4014 void Commander::estimator_check(bool *status_changed)
4015 {
4016  // Check if quality checking of position accuracy and consistency is to be performed
4017  const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
4018 
4021 
4024 
4025  const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
4026 
4027  if (_estimator_status_sub.update()) {
4028  const estimator_status_s &estimator_status = _estimator_status_sub.get();
4029 
4030  // Check for a magnetomer fault and notify the user
4031  const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
4032 
4033  if (!mag_fault_prev && mag_fault) {
4034  mavlink_log_critical(&mavlink_log_pub, "Stopping compass use! Check calibration on landing");
4035  }
4036 
4037  // Set the allowable position uncertainty based on combination of flight and estimator state
4038  // When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
4039  const bool reliant_on_opt_flow = ((estimator_status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
4040  && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
4041  && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
4042 
4043  const bool operator_controlled_position = (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL);
4044 
4045  _skip_pos_accuracy_check = reliant_on_opt_flow && operator_controlled_position;
4046 
4048  _eph_threshold_adj = INFINITY;
4049 
4050  } else {
4051  _eph_threshold_adj = _param_com_pos_fs_eph.get();
4052  }
4053 
4054  /* Check estimator status for signs of bad yaw induced post takeoff navigation failure
4055  * for a short time interval after takeoff. Fixed wing vehicles can recover using GPS heading,
4056  * but rotary wing vehicles cannot so the position and velocity validity needs to be latched
4057  * to false after failure to prevent flyaway crashes */
4058  if (run_quality_checks && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
4059 
4060  if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
4061  // reset flags and timer
4063  _nav_test_failed = false;
4064  _nav_test_passed = false;
4065 
4066  } else if (land_detector.landed) {
4067  // record time of takeoff
4069 
4070  } else {
4071  // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
4072  const bool sufficient_time = (hrt_elapsed_time(&_time_at_takeoff) > 30_s);
4073  const bool sufficient_speed = (lpos.vx * lpos.vx + lpos.vy * lpos.vy > 25.0f);
4074 
4075  bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f;
4076 
4077  if (!_nav_test_failed) {
4078  if (!_nav_test_passed) {
4079  // pass if sufficient time or speed
4080  if (sufficient_time || sufficient_speed) {
4081  _nav_test_passed = true;
4082  }
4083 
4084  // record the last time the innovation check passed
4085  if (innovation_pass) {
4087  }
4088 
4089  // if the innovation test has failed continuously, declare the nav as failed
4091  _nav_test_failed = true;
4092  mavlink_log_emergency(&mavlink_log_pub, "Critical navigation failure! Check sensor calibration");
4093  }
4094  }
4095  }
4096  }
4097  }
4098  }
4099 
4100  /* run global position accuracy checks */
4101  // Check if quality checking of position accuracy and consistency is to be performed
4102  if (run_quality_checks) {
4103  if (_nav_test_failed) {
4104  status_flags.condition_global_position_valid = false;
4105  status_flags.condition_local_position_valid = false;
4106  status_flags.condition_local_velocity_valid = false;
4107 
4108  } else {
4109  if (!_skip_pos_accuracy_check) {
4110  // use global position message to determine validity
4112  &_gpos_probation_time_us, &status_flags.condition_global_position_valid, status_changed);
4113  }
4114 
4115  // use local position message to determine validity
4117  &_lpos_probation_time_us, &status_flags.condition_local_position_valid, status_changed);
4118  check_posvel_validity(lpos.v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, &_last_lvel_fail_time_us,
4119  &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, status_changed);
4120  }
4121  }
4122 
4124  && status_flags.condition_global_position_valid) {
4125  // If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
4126  set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status);
4127  set_health_flags_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, status);
4128  }
4129 
4130  check_valid(lpos.timestamp, _param_com_pos_fs_delay.get() * 1_s, lpos.z_valid,
4131  &(status_flags.condition_local_altitude_valid), status_changed);
4132 }
4133 
4134 void
4136 {
4137  const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get();
4138 
4139  // if this is the first time entering OFFBOARD the subscription may not be active yet
4140  bool force_update = false;
4141 
4142  if (commander_state_s::MAIN_STATE_OFFBOARD) {
4143  if (offboard_control_mode.timestamp == 0) {
4145  force_update = true;
4146  }
4147  }
4148 
4149  if (_offboard_control_mode_sub.updated() || force_update) {
4150  const offboard_control_mode_s old = offboard_control_mode;
4151 
4153  if (old.ignore_thrust != offboard_control_mode.ignore_thrust ||
4154  old.ignore_attitude != offboard_control_mode.ignore_attitude ||
4155  old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x ||
4156  old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y ||
4157  old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z ||
4158  old.ignore_position != offboard_control_mode.ignore_position ||
4159  old.ignore_velocity != offboard_control_mode.ignore_velocity ||
4160  old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force ||
4161  old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) {
4162 
4163  status_changed = true;
4164  }
4165  }
4166  }
4167 
4168  if (offboard_control_mode.timestamp != 0 &&
4169  offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
4170  if (status_flags.offboard_control_signal_lost) {
4171  status_flags.offboard_control_signal_lost = false;
4172  status_flags.offboard_control_loss_timeout = false;
4173  status_changed = true;
4174  }
4175 
4176  } else {
4177  if (!status_flags.offboard_control_signal_lost) {
4178  status_flags.offboard_control_signal_lost = true;
4179  status_changed = true;
4180  }
4181 
4182  /* check timer if offboard was there but now lost */
4183  if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) {
4184  if (_param_com_of_loss_t.get() < FLT_EPSILON) {
4185  /* execute loss action immediately */
4186  status_flags.offboard_control_loss_timeout = true;
4187 
4188  } else {
4189  /* wait for timeout if set */
4190  status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
4191  OFFBOARD_TIMEOUT + _param_com_of_loss_t.get() * 1e6f < hrt_absolute_time();
4192  }
4193 
4194  if (status_flags.offboard_control_loss_timeout) {
4195  status_changed = true;
4196  }
4197  }
4198  }
4199 
4200 }
4201 
4203 {
4204  char esc_fail_msg[50];
4205  esc_fail_msg[0] = '\0';
4206 
4207  int online_bitmask = (1 << esc_status.esc_count) - 1;
4208 
4209  // Check if ALL the ESCs are online
4210  if (online_bitmask == esc_status.esc_online_flags) {
4211  status_flags.condition_escs_error = false;
4213 
4214  } else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0) {
4215 
4216  // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
4217 
4218  status_flags.condition_escs_error = true;
4219 
4220  } else if (esc_status.esc_online_flags < _last_esc_online_flags) {
4221 
4222  // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
4223 
4224  for (int index = 0; index < esc_status.esc_count; index++) {
4225  if ((esc_status.esc_online_flags & (1 << index)) == 0) {
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';
4228  }
4229  }
4230 
4231  mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg);
4232 
4234  status_flags.condition_escs_error = true;
4235  }
4236 }
4237 
4238 void usage(const char *reason)
4239 {
4240  if (reason) {
4241  PX4_INFO("%s", reason);
4242  }
4243 
4244  PRINT_MODULE_DESCRIPTION(
4245  R"DESCR_STR(
4246 ### Description
4247 The commander module contains the state machine for mode switching and failsafe behavior.
4248 )DESCR_STR");
4249 
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();
4268 }
#define VEHICLE_TYPE_FIXED_WING
uORB::Publication< commander_state_s > _commander_state_pub
Definition: Commander.hpp:275
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
Definition: Commander.hpp:251
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
static bool _last_condition_local_altitude_valid
Definition: Commander.cpp:161
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
Definition: Commander.cpp:211
uORB::SubscriptionData< vehicle_local_position_s > _local_position_sub
Definition: Commander.hpp:269
void battery_status_check()
Definition: Commander.cpp:3963
void tune_neutral(bool use_buzzer)
Blink white LED and play neutral tune (if use_buzzer == true).
static float min_stick_change
Definition: Commander.cpp:136
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)
Definition: geo.cpp:547
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL
Definition: Commander.cpp:112
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
uint8_t servo_valid
Definition: system_power.h:68
static int custom_command(int argc, char *argv[])
Definition: Commander.cpp:3697
float _battery_current
Definition: Commander.hpp:253
FailureDetector _failure_detector
Definition: Commander.hpp:185
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
uint8_t esc_online_flags
Definition: esc_status.h:65
const int64_t POSVEL_PROBATION_MIN
minimum probation duration (usec)
Definition: Commander.hpp:162
static struct vehicle_status_s status
Definition: Commander.cpp:138
State machine helper functions definitions.
uint8_t data_link_lost_counter
uint64_t timestamp
Definition: home_position.h:53
void tune_mission_fail(bool use_buzzer)
uORB::Publication< test_motor_s > _test_motor_pub
Definition: Commander.hpp:277
static constexpr uint64_t OFFBOARD_TIMEOUT
Definition: Commander.cpp:117
bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic)
uint64_t nav_state_timestamp
hrt_abstime _datalink_last_heartbeat_avoidance_system
Definition: Commander.hpp:238
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
Definition of geo / math functions to perform geodesic calculations.
bool set_home_position_alt_only()
Definition: Commander.cpp:1222
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
Definition: HealthFlags.cpp:70
bool override_enabled
Definition: safety.h:57
bool _print_avoidance_msg_once
Definition: Commander.hpp:258
bool is_fixed_wing(const struct vehicle_status_s *current_status)
uORB::Publication< actuator_armed_s > _armed_pub
Definition: Commander.hpp:274
static struct vehicle_status_flags_s status_flags
Definition: Commander.cpp:155
bool _geofence_warning_action_on
Definition: Commander.hpp:182
bool stabilization_required()
Definition: Commander.cpp:3352
uint8_t failure_detector_status
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL
Definition: Commander.cpp:120
static constexpr float STICK_ON_OFF_LIMIT
Definition: Commander.cpp:115
int do_level_calibration(orb_advert_t *mavlink_log_pub)
int main(int argc, char **argv)
Definition: main.cpp:3
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Barometer calibration routine.
__EXPORT int param_set(param_t param, const void *val)
Set the value of a parameter.
Definition: parameters.cpp:814
uint64_t timestamp
Definition: mission.h:53
Definition: I2C.hpp:51
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)
Definition: HealthFlags.cpp:76
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
#define CBRK_AIRSPD_CHK_KEY
hrt_abstime _last_gpos_fail_time_us
Last time that the global position validity recovery check failed (usec)
Definition: Commander.hpp:165
int info()
Print a little info about the driver.
Airspeed sensor calibration routine.
static struct commander_state_s internal_state
Definition: Commander.cpp:142
link_loss_actions_t
hrt_abstime _datalink_last_heartbeat_gcs
Definition: Commander.hpp:233
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
hrt_abstime _last_lpos_fail_time_us
Last time that the local position validity recovery check failed (usec)
Definition: Commander.hpp:166
void print_status()
Definition: Commander.cpp:517
void set_tune_override(int tune)
uORB::SubscriptionData< vehicle_global_position_s > _global_position_sub
Definition: Commander.hpp:268
uint8_t dataman_id
Definition: mission.h:56
bool override_available
Definition: safety.h:56
static bool _last_condition_global_position_valid
Definition: Commander.cpp:163
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]
Definition: Commander.cpp:141
static manual_control_setpoint_s sp_man
the current manual control setpoint
Definition: Commander.cpp:146
uint64_t subsystem_type
uORB::Subscription _telemetry_status_sub
Definition: Commander.hpp:231
void tune_positive(bool use_buzzer)
Blink green LED and play positive tune (if use_buzzer == true).
bool safety_switch_available
Definition: safety.h:54
LidarLite * instance
Definition: ll40ls.cpp:65
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)
Definition: Commander.cpp:581
#define FLT_EPSILON
void led_deinit()
High-resolution timer with callouts and timekeeping.
hrt_abstime _time_at_takeoff
last time we were on the ground
Definition: Commander.hpp:175
uint64_t timestamp
Definition: system_power.h:61
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
Definition: HealthFlags.cpp:44
float load
Definition: cpuload.h:54
hrt_abstime _gpos_probation_time_us
Definition: Commander.hpp:170
#define CBRK_SUPPLY_CHK_KEY
__EXPORT int commander_main(int argc, char *argv[])
The daemon app only briefly exists to start the background job.
Definition: Commander.cpp:272
bool _nav_test_failed
true if the post takeoff navigation test has failed
Definition: Commander.hpp:178
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
Definition: hysteresis.cpp:57
hrt_abstime _high_latency_datalink_heartbeat
Definition: Commander.hpp:246
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
static bool _skip_pos_accuracy_check
Definition: Commander.cpp:169
uORB::Subscription _parameter_update_sub
Definition: Commander.hpp:261
void update_control_mode()
Definition: Commander.cpp:3183
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
bool _nav_test_passed
true if the post takeoff navigation test has passed
Definition: Commander.hpp:177
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
Definition: Commander.cpp:140
static uint64_t rc_signal_lost_timestamp
Definition: Commander.cpp:157
void * commander_low_prio_loop(void *arg)
Loop that runs at a lower rate and priority for calibration and parameter tasks.
Definition: Commander.cpp:3428
void print_reject_mode(const char *msg)
Definition: Commander.cpp:3362
transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed)
Definition: Commander.cpp:2703
bool isFailure() const
void reset_posvel_validity(bool *changed)
Definition: Commander.cpp:3102
__BEGIN_DECLS void led_init()
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void estimator_check(bool *status_changed)
Definition: Commander.cpp:4014
void print_reject_arm(const char *msg)
Definition: Commander.cpp:3377
#define COMMANDER_MONITORING_LOOPSPERMSEC
Definition: Commander.cpp:113
hrt_abstime _datalink_last_heartbeat_onboard_controller
Definition: Commander.hpp:235
bool _onboard_controller_lost
Definition: Commander.hpp:236
void run() override
Definition: Commander.cpp:1241
uint16_t count
Definition: mission.h:55
bool _avoidance_system_lost
Definition: Commander.hpp:239
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.
Definition: dataman.cpp:1104
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
hrt_abstime _lvel_probation_time_us
Definition: Commander.hpp:172
bool get_state() const
Definition: hysteresis.h:60
void enable_hil()
Definition: Commander.cpp:3741
offboard_loss_actions_t
static Commander * instantiate(int argc, char *argv[])
Definition: Commander.cpp:3728
uint64_t timestamp
Definition: test_motor.h:57
static hrt_abstime commander_boot_timestamp
Definition: Commander.cpp:130
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS
Definition: uORB.h:256
uint32_t instance_count
static struct actuator_armed_s armed
Definition: Commander.cpp:139
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool _avoidance_system_status_change
Definition: Commander.hpp:241
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT
wait for hotplug sensors to come online for upto 8 seconds
Definition: Commander.cpp:118
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
static bool _last_condition_local_position_valid
Definition: Commander.cpp:162
hrt_abstime _high_latency_datalink_lost
Definition: Commander.hpp:247
uint8_t _battery_warning
Definition: Commander.hpp:252
#define warnx(...)
Definition: err.h:95
DATAMANAGER driver.
bool high_latency_data_link_lost
static int print_usage(const char *reason=nullptr)
Definition: Commander.cpp:3702
uint8_t usb_connected
Definition: system_power.h:65
uORB::SubscriptionData< offboard_control_mode_s > _offboard_control_mode_sub
Definition: Commander.hpp:267
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.
Definition: parameters.cpp:974
uORB::SubscriptionData< mission_result_s > _mission_result_sub
Definition: Commander.hpp:266
static unsigned counter
Definition: safety.c:56
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)
static volatile bool thread_should_exit
daemon exit flag
Definition: Commander.cpp:127
bool is_ground_rover(const struct vehicle_status_s *current_status)
systemlib::Hysteresis _auto_disarm_killed
Definition: Commander.hpp:256
bool updated()
Check if there is a new update.
int blink_msg_state()
static orb_advert_t mavlink_log_pub
Definition: Commander.cpp:123
transition_result_t set_main_state(const vehicle_status_s &status, bool *changed)
Definition: Commander.cpp:2692
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
systemlib::Hysteresis _auto_disarm_landed
Definition: Commander.hpp:255
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...
Definition: Commander.cpp:3785
void offboard_control_update(bool &status_changed)
Definition: Commander.cpp:4135
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
static bool last_overload
Definition: Commander.cpp:153
bool _flight_termination_triggered
Definition: Commander.hpp:186
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
bool is_safe(const safety_s &safety, const actuator_armed_s &armed)
unsigned handle_command_motor_test(const vehicle_command_s &cmd)
Definition: Commander.cpp:1123
const T & get() const
VEHICLE_MODE_FLAG
Definition: Commander.cpp:99
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
uORB::PublicationData< home_position_s > _home_pub
Definition: Commander.hpp:279
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
uint8_t esc_count
Definition: esc_status.h:63
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL
Definition: Commander.cpp:119
uint8_t main_state_t
Definition: uORB.h:258
Contains helper functions to efficiently set the system health flags from commander and preflight che...
static uint8_t arm_requirements
Definition: Commander.cpp:159
void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed)
Definition: Commander.cpp:2544
void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
Definition: hysteresis.cpp:46
void get_circuit_breaker_params()
Definition: Commander.cpp:2525
bool _geofence_violated_prev
Definition: Commander.hpp:183
#define CBRK_GPSFAIL_KEY
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)
int _last_esc_online_flags
Definition: Commander.hpp:249
void esc_status_check(const esc_status_s &esc_status)
Definition: Commander.cpp:4202
#define CBRK_USB_CHK_KEY
uORB::Publication< vehicle_status_s > _status_pub
Definition: Commander.hpp:273
Definition of esc calibration.
offboard_loss_rc_actions_t
const int64_t POSVEL_PROBATION_MAX
maximum probation duration (usec)
Definition: Commander.hpp:163
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)
Definition: preArmCheck.cpp:40
static float _eph_threshold_adj
maximum allowable horizontal position uncertainty after adjustment for flight condition ...
Definition: Commander.cpp:167
static uint8_t main_state_before_rtl
Definition: Commander.cpp:144
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)
Definition: geo.cpp:132
bool shutdown_if_allowed()
Definition: Commander.cpp:522
uORB::SubscriptionData< estimator_status_s > _estimator_status_sub
Definition: Commander.hpp:265
Remote Control calibration routine.
int32_t current_seq
Definition: mission.h:54
Definition of accelerometer calibration.
static uint64_t last_print_mode_reject_time
Definition: Commander.cpp:134
PX4 custom flight modes.
position_nav_loss_actions_t
uint8_t getStatus() const
static orb_advert_t power_button_state_pub
Definition: Commander.cpp:124
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy)
Definition: Commander.cpp:529
__EXPORT void param_reset_all(void)
Reset all parameters to their default values.
Definition: parameters.cpp:910
transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed)
Definition: Commander.cpp:2713
static bool preflight_check(bool report)
Definition: Commander.cpp:3771
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
static struct cpuload_s cpuload
Definition: Commander.cpp:151
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)
Definition: Commander.cpp:2557
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
Definition: Commander.hpp:276
static volatile bool thread_running
daemon status flag
Definition: Commander.cpp:128
hrt_abstime _lpos_probation_time_us
Definition: Commander.hpp:171
__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.
Definition: dataman.cpp:1072
bool _geofence_loiter_on
Definition: Commander.hpp:180
#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.
int buzzer_init()
#define OK
Definition: uavcan_main.cpp:71
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
Definition: Commander.cpp:148
bool _geofence_rtl_on
Definition: Commander.hpp:181
#define CBRK_FLIGHTTERM_KEY
static manual_control_setpoint_s _last_sp_man
the manual control setpoint valid at the last mode switch
Definition: Commander.cpp:147
static bool send_vehicle_command(uint16_t cmd, float param1=NAN, float param2=NAN)
Definition: Commander.cpp:251
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
Definition: Commander.hpp:272
hrt_abstime _last_lvel_fail_time_us
Last time that the local velocity validity recovery check failed (usec)
Definition: Commander.hpp:167
float ram_usage
Definition: cpuload.h:55
transition_result_t
void set_tune(int tune)
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...
Definition: geo.cpp:105
uORB::Subscription _iridiumsbd_status_sub
Definition: Commander.hpp:244
#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)
Definition: Commander.cpp:3388
#define warn(...)
Definition: err.h:94
__EXPORT int param_load_default(void)
Load parameters from the default parameter file.
bool safety_off
Definition: safety.h:55
static unsigned int leds_counter
Definition: Commander.cpp:132
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
void buzzer_deinit()
static struct vtol_vehicle_status_s vtol_status
Definition: Commander.cpp:150
uint8_t brick_valid
Definition: system_power.h:66
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)
Definition: Commander.cpp:3126
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
uint8_t _datalink_last_status_avoidance_system
Definition: Commander.hpp:242
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.
Definition: Commander.cpp:1174
void mission_init()
Definition: Commander.cpp:3746
hrt_abstime _time_last_innov_pass
last time velocity or position innovations passed
Definition: Commander.hpp:176
int px4_close(int fd)
static int task_spawn(int argc, char *argv[])
Definition: Commander.cpp:3711