PX4 Firmware
PX4 Autopilot Software http://px4.io
mc_pos_control_main.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 mc_pos_control_main.cpp
36  * Multicopter position controller.
37  */
38 
40 #include <drivers/drv_hrt.h>
44 #include <lib/mathlib/mathlib.h>
45 #include <lib/perf/perf_counter.h>
48 #include <px4_platform_common/px4_config.h>
49 #include <px4_platform_common/defines.h>
50 #include <px4_platform_common/module.h>
51 #include <px4_platform_common/module_params.h>
52 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
53 #include <px4_platform_common/posix.h>
54 #include <px4_platform_common/tasks.h>
55 #include <uORB/Publication.hpp>
57 #include <uORB/Subscription.hpp>
69 
70 #include <PositionControl.hpp>
71 #include "Takeoff.hpp"
72 
73 #include <float.h>
74 
75 using namespace time_literals;
76 
77 /**
78  * Multicopter position control app start / stop handling function
79  */
80 extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
81 
82 class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
83  public ModuleParams, public px4::WorkItem
84 {
85 public:
87 
88  ~MulticopterPositionControl() override;
89 
90  /** @see ModuleBase */
91  static int task_spawn(int argc, char *argv[]);
92 
93  /** @see ModuleBase */
94  static int custom_command(int argc, char *argv[]);
95 
96  /** @see ModuleBase */
97  static int print_usage(const char *reason = nullptr);
98 
99  void Run() override;
100 
101  bool init();
102 
103  /** @see ModuleBase::print_status() */
104  int print_status() override;
105 
106 private:
107  Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
108 
109  orb_id_t _attitude_setpoint_id{nullptr}; ///< orb metadata to publish attitude setpoint dependent if VTOL or not
110  orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; ///< attitude setpoint publication handle
111  uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
112  orb_advert_t _mavlink_log_pub{nullptr};
113 
114  uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
115  uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
116  uORB::Publication<vehicle_local_position_setpoint_s> _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */
117 
118  uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
119 
120  uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
121  uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
122  uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
123  uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
124  uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
125  uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
126 
127  hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
128 
129  int _task_failure_count{0}; /**< counter for task failures */
130 
131  vehicle_status_s _vehicle_status{}; /**< vehicle status */
132  /**< vehicle-land-detection: initialze to landed */
133  vehicle_land_detected_s _vehicle_land_detected = {
134  .timestamp = 0,
135  .alt_max = -1.0f,
136  .freefall = false,
137  .ground_contact = true,
138  .maybe_landed = true,
139  .landed = true,
140  };
141 
142  vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
143  vehicle_local_position_s _local_pos{}; /**< vehicle local position */
144  home_position_s _home_pos{}; /**< home position */
145  landing_gear_s _landing_gear{};
146  int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
147 
148  DEFINE_PARAMETERS(
149  // Position Control
150  (ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
151  (ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
152  (ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
153  (ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
154  (ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d,
155  (ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
156  (ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
157  (ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
158  (ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
159  (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
160  (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
161  (ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
162  (ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
163 
164  // Takeoff / Land
165  (ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
166  (ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
167  (ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
168  (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
169 
170  (ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
171  (ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
172  (ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
173  (ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
174  (ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
175  (ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
176  (ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
177  (ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
178  (ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max
179  );
180 
181  control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
182  control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */
183  control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */
184 
185  FlightTasks _flight_tasks; /**< class generating position controller setpoints depending on vehicle task */
186  PositionControl _control; /**< class for core PID position control */
187  PositionControlStates _states{}; /**< structure containing vehicle state information for position control */
188 
189  hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
190 
191  bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
192 
193  /** Timeout in us for trajectory data to get considered invalid */
194  static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
195  /** number of tries before switching to a failsafe flight task */
196  static constexpr int NUM_FAILURE_TRIES = 10;
197  /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */
198  static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms;
199  /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */
200  static constexpr float ALTITUDE_THRESHOLD = 0.3f;
201 
202  systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
203 
204  WeatherVane *_wv_controller{nullptr};
205  Vector3f _wv_dcm_z_sp_prev{0, 0, 1};
206 
208 
209  /**
210  * Update our local parameter cache.
211  * Parameter update can be forced when argument is true.
212  * @param force forces parameter update.
213  */
214  int parameters_update(bool force);
215 
216  /**
217  * Check for changes in subscribed topics.
218  */
219  void poll_subscriptions();
220 
221  /**
222  * Check for validity of positon/velocity states.
223  * @param vel_sp_z velocity setpoint in z-direction
224  */
225  void set_vehicle_states(const float &vel_sp_z);
226 
227  /**
228  * Limit altitude based on land-detector.
229  * @param setpoint needed to detect vehicle intention.
230  */
231  void limit_altitude(vehicle_local_position_setpoint_s &setpoint);
232 
233  /**
234  * Prints a warning message at a lowered rate.
235  * @param str the message that has to be printed.
236  */
237  void warn_rate_limited(const char *str);
238 
239  /**
240  * Publish attitude.
241  */
242  void publish_attitude();
243 
244  /**
245  * Adjust the setpoint during landing.
246  * Thrust is adjusted to support the land-detector during detection.
247  * @param setpoint gets adjusted based on land-detector state
248  */
249  void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint);
250 
251  /**
252  * Start flightasks based on navigation state.
253  * This methods activates a task based on the navigation state.
254  */
255  void start_flight_task();
256 
257  /**
258  * Failsafe.
259  * If flighttask fails for whatever reason, then do failsafe. This could
260  * occur if the commander fails to switch to a mode in case of invalid states or
261  * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set
262  * to true, the failsafe will be initiated immediately.
263  */
264  void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
265  const bool warn);
266 
267  /**
268  * Reset setpoints to NAN
269  */
270  void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
271 
272  /**
273  * check if task should be switched because of failsafe
274  */
275  void check_failure(bool task_failure, uint8_t nav_state);
276 
277  /**
278  * send vehicle command to inform commander about failsafe
279  */
280  void send_vehicle_cmd_do(uint8_t nav_state);
281 };
282 
284  SuperBlock(nullptr, "MPC"),
285  ModuleParams(nullptr),
286  WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
287  _vel_x_deriv(this, "VELD"),
288  _vel_y_deriv(this, "VELD"),
289  _vel_z_deriv(this, "VELD"),
290  _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
291 {
292  // fetch initial parameter values
293  parameters_update(true);
294 
295  // set failsafe hysteresis
297 }
298 
300 {
301  delete _wv_controller;
302 
304 }
305 
306 bool
308 {
310  PX4_ERR("vehicle_local_position callback registration failed!");
311  return false;
312  }
313 
314  _local_pos_sub.set_interval_us(20_ms); // 50 Hz max update rate
315 
317 
318  return true;
319 }
320 
321 void
323 {
325 
326  if (now - _last_warn > 200_ms) {
327  PX4_WARN("%s", string);
328  _last_warn = now;
329  }
330 }
331 
332 int
334 {
335  // check for parameter updates
336  if (_parameter_update_sub.updated() || force) {
337  // clear update
338  parameter_update_s pupdate;
339  _parameter_update_sub.copy(&pupdate);
340 
341  // update parameters from storage
342  ModuleParams::updateParams();
343  SuperBlock::updateParams();
344 
345  _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get()));
346  _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()),
347  Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()),
348  Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()));
349  _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
350  _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
351  _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
352  _control.setHoverThrust(_param_mpc_thr_hover.get());
353 
354  // Check that the design parameters are inside the absolute maximum constraints
355  if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
356  _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get());
357  _param_mpc_xy_cruise.commit();
358  mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed");
359  }
360 
361  if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) {
362  _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get());
363  _param_mpc_vel_manual.commit();
364  mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed");
365  }
366 
367  if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
368  _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
369  _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
370  _param_mpc_thr_max.get()));
371  _param_mpc_thr_hover.commit();
372  mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
373  }
374 
376 
377  // initialize vectors from params and enforce constraints
378  _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
379  _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
380 
381  // set trigger time for takeoff delay
382  _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
383  _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
384  _takeoff.generateInitialRampValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
385 
386  if (_wv_controller != nullptr) {
388  }
389  }
390 
391  return OK;
392 }
393 
394 void
396 {
398 
399  // set correct uORB ID, depending on if vehicle is VTOL or not
400  if (!_attitude_setpoint_id) {
401  if (_vehicle_status.is_vtol) {
402  _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
403 
404  } else {
405  _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
406  }
407  }
408 
409  // if vehicle is a VTOL we want to enable weathervane capabilities
410  if (_wv_controller == nullptr && _vehicle_status.is_vtol) {
411  _wv_controller = new WeatherVane();
412  }
413  }
414 
418 
419  if (_att_sub.updated()) {
420  vehicle_attitude_s att;
421 
422  if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
423  _states.yaw = Eulerf(Quatf(att.q)).psi();
424  }
425  }
426 }
427 
428 void
430 {
432  // there is no altitude limitation present or the required information not available
433  return;
434  }
435 
436  // maximum altitude == minimal z-value (NED)
437  const float min_z = _home_pos.z + (-_vehicle_land_detected.alt_max);
438 
439  if (_states.position(2) < min_z) {
440  // above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
441  setpoint.z = min_z;
442  setpoint.vz = math::max(setpoint.vz, 0.f);
443  }
444 }
445 
446 void
448 {
449  if (_local_pos.timestamp == 0) {
450  return;
451  }
452 
453  // only set position states if valid and finite
454  if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) {
457 
458  } else {
459  _states.position(0) = _states.position(1) = NAN;
460  }
461 
462  if (PX4_ISFINITE(_local_pos.z) && _local_pos.z_valid) {
464 
465  } else {
466  _states.position(2) = NAN;
467  }
468 
469  if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) {
474 
475  } else {
476  _states.velocity(0) = _states.velocity(1) = NAN;
478 
479  // since no valid velocity, update derivate with 0
480  _vel_x_deriv.update(0.0f);
481  _vel_y_deriv.update(0.0f);
482  }
483 
484  if (_param_mpc_alt_mode.get() && _local_pos.dist_bottom_valid && PX4_ISFINITE(_local_pos.dist_bottom_rate)) {
485  // terrain following
488 
489  } else if (PX4_ISFINITE(_local_pos.vz)) {
490 
492 
493  if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
494  // A change in velocity is demanded. Set velocity to the derivative of position
495  // because it has less bias but blend it in across the landing speed range
496  float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f);
497  _states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
498  }
499 
501 
502  } else {
503  _states.velocity(2) = _states.acceleration(2) = NAN;
504  // since no valid velocity, update derivate with 0
505  _vel_z_deriv.update(0.0f);
506 
507  }
508 }
509 
510 int
512 {
514  PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask());
515 
516  } else {
517  PX4_INFO("Running, no flight task active");
518  }
519 
521 
522  return 0;
523 }
524 
525 void
527 {
528  if (should_exit()) {
530  exit_and_cleanup();
531  return;
532  }
533 
535 
537 
539  parameters_update(false);
540 
541  // set _dt in controllib Block - the time difference since the last loop iteration in seconds
542  const hrt_abstime time_stamp_now = hrt_absolute_time();
543  setDt((time_stamp_now - _time_stamp_last_loop) / 1e6f);
544  _time_stamp_last_loop = time_stamp_now;
545 
546  const bool was_in_failsafe = _in_failsafe;
547  _in_failsafe = false;
548 
549  // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
550  // that turns the nose of the vehicle into the wind
551  if (_wv_controller != nullptr) {
552 
553  // in manual mode we just want to use weathervane if position is controlled as well
554  // in mission, enabling wv is done in flight task
558 
559  } else {
561  }
562  }
563 
565  }
566 
567  // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
570 
571  // switch to the required flighttask
573 
574  // check if any task is active
576  // setpoints and constraints for the position controller from flighttask or failsafe
579 
581 
582  // update task
583  if (!_flight_tasks.update()) {
584  // FAILSAFE
585  // Task was not able to update correctly. Do Failsafe.
586  failsafe(setpoint, _states, false, !was_in_failsafe);
587 
588  } else {
589  setpoint = _flight_tasks.getPositionSetpoint();
590  constraints = _flight_tasks.getConstraints();
591 
592  _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
593 
594  // Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
595  if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
596  !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
597  !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
598  failsafe(setpoint, _states, true, !was_in_failsafe);
599  }
600 
601  // Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
602  // of these setpoints are valid
603  if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
604  failsafe(setpoint, _states, true, !was_in_failsafe);
605  }
606  }
607 
608  // publish trajectory setpoint
609  _traj_sp_pub.publish(setpoint);
610 
612 
613  // check if all local states are valid and map accordingly
614  set_vehicle_states(setpoint.vz);
615 
616  // handle smooth takeoff
618  constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
619  constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
620 
621  if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
622  // we are not flying yet and need to avoid any corrections
623  reset_setpoint_to_nan(setpoint);
624  setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
625  // set yaw-sp to current yaw
626  // TODO: we need a clean way to disable yaw control
627  setpoint.yaw = _states.yaw;
628  setpoint.yawspeed = 0.f;
629  // prevent any integrator windup
632  // reactivate the task which will reset the setpoint to current state
634  }
635 
636  if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
637  constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
638  }
639 
640  // limit altitude only if local position is valid
641  if (PX4_ISFINITE(_states.position(2))) {
642  limit_altitude(setpoint);
643  }
644 
645  // Update states, setpoints and constraints.
646  _control.updateConstraints(constraints);
648 
649  // update position controller setpoints
650  if (!_control.updateSetpoint(setpoint)) {
651  warn_rate_limited("Position-Control Setpoint-Update failed");
652  failsafe(setpoint, _states, true, !was_in_failsafe);
653  _control.updateSetpoint(setpoint);
654  constraints = FlightTask::empty_constraints;
655  }
656 
657  // Generate desired thrust and yaw.
659 
660  // Fill local position, velocity and thrust setpoint.
661  // This message contains setpoints where each type of setpoint is either the input to the PositionController
662  // or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).
663  // Example:
664  // If the desired setpoint is position-setpoint, _local_pos_sp will contain
665  // position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller.
666  // If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint
667  // will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the
668  // PositionController.
669  vehicle_local_position_setpoint_s local_pos_sp{};
670  local_pos_sp.timestamp = time_stamp_now;
671  _control.getLocalPositionSetpoint(local_pos_sp);
672  // Temporary setpoint message adjustments while PositionControl class is still keeping involved internal setpoints
673  local_pos_sp.x = setpoint.x;
674  local_pos_sp.y = setpoint.y;
675  local_pos_sp.z = setpoint.z;
676  local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
677  local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
678  local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
679 
680  // Publish local position setpoint
681  // This message will be used by other modules (such as Landdetector) to determine
682  // vehicle intention.
683  _local_pos_sp_pub.publish(local_pos_sp);
684 
685  // Inform FlightTask about the input and output of the velocity controller
686  // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
688 
689  vehicle_attitude_setpoint_s attitude_setpoint{};
690  attitude_setpoint.timestamp = time_stamp_now;
691  _control.getAttitudeSetpoint(attitude_setpoint);
692 
693  // Part of landing logic: if ground-contact/maybe landed was detected, turn off
694  // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
695  // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.
696  if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
697  limit_thrust_during_landing(attitude_setpoint);
698  }
699 
700  // publish attitude setpoint
701  // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
702  // Not publishing when not running a flight task
703  // in stabilized mode attitude setpoints get ignored
704  // in offboard with attitude setpoints they come from MAVLink directly
705  if (_attitude_setpoint_id != nullptr) {
707  }
708 
709  _wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
710 
711  // if there's any change in landing gear setpoint publish it
713  && gear.landing_gear != landing_gear_s::GEAR_KEEP) {
714 
716  _landing_gear.timestamp = time_stamp_now;
717 
719  }
720 
722 
723  } else {
724  // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
728  }
729  }
730 
732 }
733 
734 void
736 {
737  bool task_failure = false;
738  bool should_disable_task = true;
739  int prev_failure_count = _task_failure_count;
740 
741  // Do not run any flight task for VTOLs in fixed-wing mode
743  _flight_tasks.switchTask(FlightTaskIndex::None);
744  return;
745  }
746 
748  should_disable_task = false;
749  FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition);
750 
751  if (error != FlightTaskError::NoError) {
752  if (prev_failure_count == 0) {
753  PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error));
754  }
755 
756  task_failure = true;
758 
759  } else {
760  // we want to be in this mode, reset the failure count
762  }
763 
764  return;
765  }
766 
767  // offboard
768  if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
774 
775  should_disable_task = false;
776  FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
777 
778  if (error != FlightTaskError::NoError) {
779  if (prev_failure_count == 0) {
780  PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error));
781  }
782 
783  task_failure = true;
785 
786  } else {
787  // we want to be in this mode, reset the failure count
789  }
790  }
791 
792  // Auto-follow me
793  if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
794  should_disable_task = false;
795  FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe);
796 
797  if (error != FlightTaskError::NoError) {
798  if (prev_failure_count == 0) {
799  PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error));
800  }
801 
802  task_failure = true;
804 
805  } else {
806  // we want to be in this mode, reset the failure count
808  }
809 
811  // Auto related maneuvers
812  should_disable_task = false;
814 
815  switch (_param_mpc_auto_mode.get()) {
816  case 1:
817  error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
818  break;
819 
820  default:
821  error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
822  break;
823  }
824 
825  if (error != FlightTaskError::NoError) {
826  if (prev_failure_count == 0) {
827  PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error));
828  }
829 
830  task_failure = true;
832 
833  } else {
834  // we want to be in this mode, reset the failure count
836  }
837 
838  } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
839 
840  // Emergency descend
841  should_disable_task = false;
843 
844  error = _flight_tasks.switchTask(FlightTaskIndex::Descend);
845 
846  if (error != FlightTaskError::NoError) {
847  if (prev_failure_count == 0) {
848  PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error));
849  }
850 
851  task_failure = true;
853 
854  } else {
855  // we want to be in this mode, reset the failure count
857  }
858 
859  }
860 
861  // manual position control
862  if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) {
863  should_disable_task = false;
865 
866  switch (_param_mpc_pos_mode.get()) {
867  case 1:
868  error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
869  break;
870 
871  case 2:
872  error = _flight_tasks.switchTask(FlightTaskIndex::Sport);
873  break;
874 
875  case 3:
876  error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
877  break;
878 
879  default:
880  error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
881  break;
882  }
883 
884  if (error != FlightTaskError::NoError) {
885  if (prev_failure_count == 0) {
886  PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
887  }
888 
889  task_failure = true;
891 
892  } else {
893  check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL);
894  task_failure = false;
895  }
896  }
897 
898  // manual altitude control
899  if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) {
900  should_disable_task = false;
902 
903  switch (_param_mpc_pos_mode.get()) {
904  case 1:
905  error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth);
906  break;
907 
908  case 3:
909  error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);
910  break;
911 
912  default:
913  error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
914  break;
915  }
916 
917  if (error != FlightTaskError::NoError) {
918  if (prev_failure_count == 0) {
919  PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
920  }
921 
922  task_failure = true;
924 
925  } else {
926  check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL);
927  task_failure = false;
928  }
929  }
930 
931  if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
932  should_disable_task = false;
933  }
934 
935  // check task failure
936  if (task_failure) {
937 
938  // for some reason no flighttask was able to start.
939  // go into failsafe flighttask
940  FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe);
941 
942  if (error != FlightTaskError::NoError) {
943  // No task was activated.
944  _flight_tasks.switchTask(FlightTaskIndex::None);
945  }
946 
947  } else if (should_disable_task) {
948  _flight_tasks.switchTask(FlightTaskIndex::None);
949  }
950 }
951 
952 void
954 {
957  // we set the collective thrust to zero, this will help to decide if we are actually landed or not
958  setpoint.thrust_body[2] = 0.f;
959  // go level to avoid corrections but keep the heading we have
960  Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);
961  setpoint.yaw_sp_move_rate = 0.f;
962  // prevent any position control integrator windup
965  }
966 }
967 
968 void
970  const bool force, const bool warn)
971 {
973 
974  if (!_failsafe_land_hysteresis.get_state() && !force) {
975  // just keep current setpoint and don't do anything.
976 
977  } else {
978  reset_setpoint_to_nan(setpoint);
979 
980  if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
981  // don't move along xy
982  setpoint.vx = setpoint.vy = 0.0f;
983 
984  if (warn) {
985  PX4_WARN("Failsafe: stop and wait");
986  }
987 
988  } else {
989  // descend with land speed since we can't stop
990  setpoint.thrust[0] = setpoint.thrust[1] = 0.f;
991  setpoint.vz = _param_mpc_land_speed.get();
992 
993  if (warn) {
994  PX4_WARN("Failsafe: blind land");
995  }
996  }
997 
998  if (PX4_ISFINITE(_states.velocity(2))) {
999  // don't move along z if we can stop in all dimensions
1000  if (!PX4_ISFINITE(setpoint.vz)) {
1001  setpoint.vz = 0.f;
1002  }
1003 
1004  } else {
1005  // emergency descend with a bit below hover thrust
1006  setpoint.vz = NAN;
1007  setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f;
1008 
1009  if (warn) {
1010  PX4_WARN("Failsafe: blind descend");
1011  }
1012  }
1013 
1014  _in_failsafe = true;
1015  }
1016 }
1017 
1018 void
1020 {
1021  setpoint.x = setpoint.y = setpoint.z = NAN;
1022  setpoint.vx = setpoint.vy = setpoint.vz = NAN;
1023  setpoint.yaw = setpoint.yawspeed = NAN;
1024  setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
1025  setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
1026 }
1027 
1028 void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_state)
1029 {
1030  if (!task_failure) {
1031  // we want to be in this mode, reset the failure count
1032  _task_failure_count = 0;
1033 
1034  } else if (_task_failure_count > NUM_FAILURE_TRIES) {
1035  // tell commander to switch mode
1036  PX4_WARN("Previous flight task failed, switching to mode %d", nav_state);
1037  send_vehicle_cmd_do(nav_state);
1038  _task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration
1039  }
1040 }
1041 
1043 {
1045  command.timestamp = hrt_absolute_time();
1046  command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
1047  command.param1 = (float)1; // base mode
1048  command.param3 = (float)0; // sub mode
1049  command.target_system = 1;
1050  command.target_component = 1;
1051  command.source_system = 1;
1052  command.source_component = 1;
1053  command.confirmation = false;
1054  command.from_external = false;
1055 
1056  // set the main mode
1057  switch (nav_state) {
1058  case vehicle_status_s::NAVIGATION_STATE_STAB:
1059  command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED;
1060  break;
1061 
1062  case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
1063  command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL;
1064  break;
1065 
1066  case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
1067  command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
1068  command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
1069  break;
1070 
1071  default: //vehicle_status_s::NAVIGATION_STATE_POSCTL
1072  command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL;
1073  break;
1074  }
1075 
1076  // publish the vehicle command
1078 }
1079 
1080 int MulticopterPositionControl::task_spawn(int argc, char *argv[])
1081 {
1083 
1084  if (instance) {
1085  _object.store(instance);
1086  _task_id = task_id_is_work_queue;
1087 
1088  if (instance->init()) {
1089  return PX4_OK;
1090  }
1091 
1092  } else {
1093  PX4_ERR("alloc failed");
1094  }
1095 
1096  delete instance;
1097  _object.store(nullptr);
1098  _task_id = -1;
1099 
1100  return PX4_ERROR;
1101 }
1102 
1104 {
1105  return print_usage("unknown command");
1106 }
1107 
1109 {
1110  if (reason) {
1111  PX4_WARN("%s\n", reason);
1112  }
1113 
1114  PRINT_MODULE_DESCRIPTION(
1115  R"DESCR_STR(
1116 ### Description
1117 The controller has two loops: a P loop for position error and a PID loop for velocity error.
1118 Output of the velocity controller is thrust vector that is split to thrust direction
1119 (i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself).
1120 
1121 The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and
1122 logging.
1123 )DESCR_STR");
1124 
1125  PRINT_MODULE_USAGE_NAME("mc_pos_control", "controller");
1126  PRINT_MODULE_USAGE_COMMAND("start");
1127  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
1128 
1129  return 0;
1130 }
1131 
1132 int mc_pos_control_main(int argc, char *argv[])
1133 {
1134  return MulticopterPositionControl::main(argc, argv);
1135 }
hrt_abstime _time_stamp_last_loop
time stamp of last loop iteration
#define VEHICLE_TYPE_FIXED_WING
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
static const vehicle_local_position_setpoint_s empty_setpoint
Empty setpoint.
Definition: FlightTask.hpp:139
Takeoff _takeoff
state machine and ramp to bring the vehicle off the ground without jumps
void generateInitialRampValue(const float hover_thrust, const float velocity_p_gain)
Calculate a vertical velocity to initialize the takeoff ramp that when passed to the velocity control...
Definition: Takeoff.cpp:41
measure the time elapsed performing an event
Definition: perf_counter.h:56
const vehicle_local_position_setpoint_s getPositionSetpoint()
Get the output data from the current task.
Definition: FlightTasks.cpp:29
int parameters_update(bool force)
Update our local parameter cache.
bool update(void *dst)
Copy the struct if updated.
static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND
If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land...
control::BlockDerivative _vel_x_deriv
velocity derivative in x
void resetIntegralZ()
Set the integral term in z to 0.
void updateConstraints(const vehicle_constraints_s &constraints)
Set constraints that are stricter than the global limits.
uORB::Subscription _vehicle_land_detected_sub
vehicle land detected subscription
uORB::SubscriptionCallbackWorkItem _local_pos_sub
vehicle local position
const vehicle_constraints_s getConstraints()
Get task dependent constraints.
Definition: FlightTasks.cpp:39
int main(int argc, char **argv)
Definition: main.cpp:3
void check_failure(bool task_failure, uint8_t nav_state)
check if task should be switched because of failsafe
control::BlockDerivative _vel_z_deriv
velocity derivative in z
void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint)
Adjust the setpoint during landing.
Definition: I2C.hpp:51
void resetIntegralXY()
Set the integral term in xy to 0.
matrix::Vector3f acceleration
bool publish(const T &data)
Publish the struct.
void print_status()
Definition: Commander.cpp:517
void generateThrustYawSetpoint(const float dt)
Apply P-position and PID-velocity controller that updates the member thrust, yaw- and yawspeed-setpoi...
void activate()
Definition: WeatherVane.hpp:55
static void print_usage()
A cascaded position controller for position/velocity control only.
void set_interval_us(uint32_t interval)
Set the interval in microseconds.
matrix::Vector3f position
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
Get the controllers output attitude setpoint This attitude setpoint was generated from the resulting ...
LidarLite * instance
Definition: ll40ls.cpp:65
FlightTaskError
Definition: FlightTasks.hpp:54
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
Reset setpoints to NAN.
float _dt
Definition: Block.hpp:89
void deactivate()
Definition: WeatherVane.hpp:57
uORB::Subscription _control_mode_sub
vehicle control mode subscription
#define FLT_EPSILON
High-resolution timer with callouts and timekeeping.
orb_advert_t _vehicle_attitude_setpoint_pub
attitude setpoint publication handle
__EXPORT int mc_pos_control_main(int argc, char *argv[])
Multicopter position control app start / stop handling function.
bool update()
Call regularly in the control loop cycle to execute the task.
Definition: FlightTasks.cpp:18
float updateRamp(const float dt, const float takeoff_desired_vz)
Update and return the velocity constraint ramp value during takeoff.
Definition: Takeoff.cpp:111
void updateState(const PositionControlStates &states)
Update the current vehicle state.
void setTiltLimit(const float tilt)
Set the maximum tilt angle in radians the output attitude is allowed to have.
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, const bool warn)
Failsafe.
vehicle_local_position_s _local_pos
vehicle local position
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
Update the state for the takeoff.
Definition: Takeoff.cpp:47
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
Definition: hysteresis.cpp:57
void setYawHandler(WeatherVane *ext_yaw_handler)
Sets an external yaw handler.
orb_id_t _attitude_setpoint_id
orb metadata to publish attitude setpoint dependent if VTOL or not
void limit_altitude(vehicle_local_position_setpoint_s &setpoint)
Limit altitude based on land-detector.
uORB::PublicationQueued< vehicle_command_s > _pub_vehicle_command
vehicle command publication
uORB::Subscription _vehicle_status_sub
vehicle status subscription
PositionControl _control
class for core PID position control
static constexpr int NUM_FAILURE_TRIES
number of tries before switching to a failsafe flight task
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uORB::Subscription _parameter_update_sub
notification of parameter updates
home_position_s _home_pos
home position
void reActivate()
This method will re-activate current task.
void setTakeoffRampTime(const float seconds)
Definition: Takeoff.hpp:62
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
void poll_subscriptions()
Check for changes in subscribed topics.
Header common to all counters.
bool get_state() const
Definition: hysteresis.h:60
static void parameters_update()
update all parameters
void perf_free(perf_counter_t handle)
Free a counter.
void setThrustLimits(const float min, const float max)
Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller...
void init()
Activates/configures the hardware registers.
void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down)
Set the maximum velocity to execute with feed forward and position control.
void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw)
Definition: WeatherVane.cpp:48
void start_flight_task()
Start flightasks based on navigation state.
void setDt(float dt) override
Definition: Block.cpp:100
void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp)
Core Position-Control for MC.
perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name)
Get the reference to an existing counter or create a new one if it does not exist.
static int custom_command(int argc, char *argv[])
uORB::Publication< vehicle_local_position_setpoint_s > _local_pos_sp_pub
vehicle local position setpoint publication
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
uORB::Subscription _home_pos_sub
home position
int8_t landing_gear
Definition: landing_gear.h:57
matrix::Vector3f velocity
void setHoverThrust(const float thrust)
Set the maximum tilt angle in radians the output attitude is allowed to have.
Euler< float > Eulerf
Definition: Euler.hpp:156
void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
Set the velocity control gains.
const matrix::Vector3f getVelSp() const
Get the.
bool isAnyTaskActive() const
Check if any task is active.
void perf_end(perf_counter_t handle)
End a performance event.
bool updated()
Check if there is a new update.
void update_parameters()
Definition: WeatherVane.hpp:67
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
uORB::Publication< landing_gear_s > _landing_gear_pub
void set_vehicle_states(const float &vel_sp_z)
Check for validity of positon/velocity states.
static int print_usage(const char *reason=nullptr)
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
void setSpoolupTime(const float seconds)
Definition: Takeoff.hpp:63
void setPositionGains(const matrix::Vector3f &P)
Set the position control gains.
float update(float input)
Update the state and get current derivative.
Hysteresis of a boolean value.
Vector3< float > Vector3f
Definition: Vector3.hpp:136
int _task_failure_count
counter for task failures
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US
Timeout in us for trajectory data to get considered invalid.
void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
Definition: hysteresis.cpp:46
Object metadata.
Definition: uORB.h:50
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
hrt_abstime _last_warn
timer when the last warn message was sent out
uint64_t timestamp
Definition: landing_gear.h:56
systemlib::Hysteresis _failsafe_land_hysteresis
becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND
bool weathervane_enabled()
Definition: WeatherVane.hpp:61
vehicle_land_detected_s _vehicle_land_detected
Controller library code.
bool _in_failsafe
true if failsafe was entered within current cycle
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
Get the controllers output local position setpoint These setpoints are the ones which were executed o...
PositionControlStates _states
structure containing vehicle state information for position control
vehicle_status_s _vehicle_status
vehicle status
void handleParameterUpdate()
Call this whenever a parameter update notification is received (parameter_update uORB message) ...
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
PX4 custom flight modes.
bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
Update the desired setpoints.
A class handling all takeoff states and a smooth ramp up of the motors.
A simple derivative approximation.
int getActiveTask() const
Get the number of the active task.
Definition: bst.cpp:62
FlightTaskError switchTask()
Switch to the next task in the available list (for testing)
#define OK
Definition: uavcan_main.cpp:71
FlightTasks _flight_tasks
class generating position controller setpoints depending on vehicle task
bool update(void *dst)
Update the struct.
void send_vehicle_cmd_do(uint8_t nav_state)
send vehicle command to inform commander about failsafe
vehicle_control_mode_s _control_mode
vehicle control mode
void warn_rate_limited(const char *str)
Prints a warning message at a lowered rate.
const char * errorToString(const FlightTaskError error)
Call this method to get the description of a task error.
control::BlockDerivative _vel_y_deriv
velocity derivative in y
const landing_gear_s getGear()
Get landing gear position.
Definition: FlightTasks.cpp:49
static const vehicle_constraints_s empty_constraints
Empty constraints.
Definition: FlightTask.hpp:145
static int task_spawn(int argc, char *argv[])
TakeoffState getTakeoffState()
Definition: Takeoff.hpp:91
bool copy(void *dst)
Copy the struct.
#define warn(...)
Definition: err.h:94
void perf_begin(perf_counter_t handle)
Begin a performance event.
uORB::Subscription _att_sub
vehicle attitude
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
Performance measuring tools.
uORB::Publication< vehicle_local_position_setpoint_s > _traj_sp_pub
trajectory setpoints publication