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> 83 public ModuleParams,
public px4::WorkItem
91 static int task_spawn(
int argc,
char *argv[]);
94 static int custom_command(
int argc,
char *argv[]);
97 static int print_usage(
const char *reason =
nullptr);
129 int _task_failure_count{0};
137 .ground_contact =
true,
138 .maybe_landed =
true,
146 int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
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,
165 (ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time,
166 (ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t,
167 (ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
168 (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
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,
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,
177 (ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
178 (ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max
191 bool _in_failsafe =
false;
196 static constexpr
int NUM_FAILURE_TRIES = 10;
198 static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms;
200 static constexpr
float ALTITUDE_THRESHOLD = 0.3f;
219 void poll_subscriptions();
225 void set_vehicle_states(
const float &vel_sp_z);
237 void warn_rate_limited(
const char *str);
242 void publish_attitude();
255 void start_flight_task();
275 void check_failure(
bool task_failure, uint8_t nav_state);
280 void send_vehicle_cmd_do(uint8_t nav_state);
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"),
310 PX4_ERR(
"vehicle_local_position callback registration failed!");
327 PX4_WARN(
"%s",
string);
342 ModuleParams::updateParams();
343 SuperBlock::updateParams();
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()));
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();
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();
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();
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()));
496 float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f);
517 PX4_INFO(
"Running, no flight task active");
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]))) {
603 if (!PX4_ISFINITE(setpoint.
z) && !PX4_ISFINITE(setpoint.
vz) && !PX4_ISFINITE(setpoint.
thrust[2])) {
673 local_pos_sp.x = setpoint.
x;
674 local_pos_sp.y = setpoint.
y;
675 local_pos_sp.z = setpoint.
z;
690 attitude_setpoint.
timestamp = time_stamp_now;
737 bool task_failure =
false;
738 bool should_disable_task =
true;
748 should_disable_task =
false;
752 if (prev_failure_count == 0) {
775 should_disable_task =
false;
779 if (prev_failure_count == 0) {
794 should_disable_task =
false;
798 if (prev_failure_count == 0) {
812 should_disable_task =
false;
815 switch (_param_mpc_auto_mode.get()) {
826 if (prev_failure_count == 0) {
841 should_disable_task =
false;
847 if (prev_failure_count == 0) {
863 should_disable_task =
false;
866 switch (_param_mpc_pos_mode.get()) {
885 if (prev_failure_count == 0) {
893 check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL);
894 task_failure =
false;
900 should_disable_task =
false;
903 switch (_param_mpc_pos_mode.get()) {
918 if (prev_failure_count == 0) {
926 check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL);
927 task_failure =
false;
932 should_disable_task =
false;
947 }
else if (should_disable_task) {
970 const bool force,
const bool warn)
982 setpoint.
vx = setpoint.
vy = 0.0f;
985 PX4_WARN(
"Failsafe: stop and wait");
991 setpoint.
vz = _param_mpc_land_speed.get();
994 PX4_WARN(
"Failsafe: blind land");
1000 if (!PX4_ISFINITE(setpoint.
vz)) {
1007 setpoint.
thrust[2] = _param_mpc_thr_hover.get() * .8f;
1010 PX4_WARN(
"Failsafe: blind descend");
1021 setpoint.
x = setpoint.
y = setpoint.
z = NAN;
1022 setpoint.
vx = setpoint.
vy = setpoint.
vz = NAN;
1030 if (!task_failure) {
1036 PX4_WARN(
"Previous flight task failed, switching to mode %d", nav_state);
1046 command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
1054 command.from_external =
false;
1057 switch (nav_state) {
1058 case vehicle_status_s::NAVIGATION_STATE_STAB:
1062 case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
1066 case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
1085 _object.store(instance);
1086 _task_id = task_id_is_work_queue;
1088 if (instance->
init()) {
1093 PX4_ERR(
"alloc failed");
1097 _object.store(
nullptr);
1111 PX4_WARN(
"%s\n", reason);
1114 PRINT_MODULE_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). 1121 The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and 1125 PRINT_MODULE_USAGE_NAME("mc_pos_control",
"controller");
1126 PRINT_MODULE_USAGE_COMMAND(
"start");
1127 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
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)
landing_gear_s _landing_gear
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
static const vehicle_local_position_setpoint_s empty_setpoint
Empty setpoint.
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...
measure the time elapsed performing an event
const vehicle_local_position_setpoint_s getPositionSetpoint()
Get the output data from the current task.
int parameters_update(bool force)
Update our local parameter cache.
bool flag_control_auto_enabled
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.
bool flag_control_climb_rate_enabled
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.
int main(int argc, char **argv)
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.
void resetIntegralXY()
Set the integral term in xy to 0.
matrix::Vector3f acceleration
bool publish(const T &data)
Publish the struct.
void generateThrustYawSetpoint(const float dt)
Apply P-position and PID-velocity controller that updates the member thrust, yaw- and yawspeed-setpoi...
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 ...
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
Reset setpoints to NAN.
uORB::Subscription _control_mode_sub
vehicle control mode subscription
High-resolution timer with callouts and timekeeping.
int print_status() override
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.
float updateRamp(const float dt, const float takeoff_desired_vz)
Update and return the velocity constraint ramp value during takeoff.
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.
void set_state_and_update(const bool new_state, const hrt_abstime &now_us)
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.
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)
bool publish(const T &data)
Publish the struct.
bool flag_control_velocity_enabled
bool flag_control_manual_enabled
void poll_subscriptions()
Check for changes in subscribed topics.
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.
orb_advert_t _mavlink_log_pub
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)
void start_flight_task()
Start flightasks based on navigation state.
void setDt(float dt) override
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)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
uORB::Subscription _home_pos_sub
home position
matrix::Vector3f velocity
void setHoverThrust(const float thrust)
Set the maximum tilt angle in radians the output attitude is allowed to have.
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.
int8_t _old_landing_gear_position
bool flag_control_acceleration_enabled
bool updated()
Check if there is a new update.
perf_counter_t _cycle_perf
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
~MulticopterPositionControl() override
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
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)
bool flag_control_altitude_enabled
void setSpoolupTime(const float seconds)
void setPositionGains(const matrix::Vector3f &P)
Set the position control gains.
WeatherVane * _wv_controller
float update(float input)
Update the state and get current derivative.
Hysteresis of a boolean value.
Vector3< float > Vector3f
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)
bool flag_control_position_enabled
constexpr _Tp max(_Tp a, _Tp b)
hrt_abstime _last_warn
timer when the last warn message was sent out
systemlib::Hysteresis _failsafe_land_hysteresis
becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND
bool weathervane_enabled()
vehicle_land_detected_s _vehicle_land_detected
bool _in_failsafe
true if failsafe was entered within current cycle
Quaternion< float > Quatf
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.
MulticopterPositionControl()
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.
void unregisterCallback()
int getActiveTask() const
Get the number of the active task.
FlightTaskError switchTask()
Switch to the next task in the available list (for testing)
Vector3f _wv_dcm_z_sp_prev
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.
static const vehicle_constraints_s empty_constraints
Empty constraints.
static int task_spawn(int argc, char *argv[])
TakeoffState getTakeoffState()
bool copy(void *dst)
Copy the struct.
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.
Performance measuring tools.
uORB::Publication< vehicle_local_position_setpoint_s > _traj_sp_pub
trajectory setpoints publication