46 ModuleParams(nullptr),
47 WorkItem(MODULE_NAME,
px4::wq_configurations::rate_ctrl),
64 PX4_ERR(
"vehicle_angular_velocity callback registration failed!");
77 const Vector3f rate_k =
Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
80 rate_k.
emult(
Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
81 rate_k.
emult(
Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
82 rate_k.
emult(
Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));
85 Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
90 Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
95 radians(_param_mc_acro_y_max.get()));
128 float landing_gear = landing_gear_s::GEAR_DOWN;
131 landing_gear = landing_gear_s::GEAR_UP;
191 bool manual_rate_sp =
false;
207 manual_rate_sp =
true;
222 if (manual_rate_sp) {
223 if (manual_control_updated) {
239 v_rates_sp.thrust_body[0] = 0.0f;
240 v_rates_sp.thrust_body[1] = 0.0f;
304 actuators.
control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
305 actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
306 actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
307 actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(
_thrust_sp) ?
_thrust_sp : 0.0f;
312 if (_param_mc_bat_scale_en.get()) {
322 for (
int i = 0; i < 4; i++) {
349 _object.store(instance);
350 _task_id = task_id_is_work_queue;
352 if (instance->
init()) {
357 PX4_ERR(
"alloc failed");
361 _object.store(
nullptr);
384 PX4_WARN(
"%s\n", reason);
387 PRINT_MODULE_DESCRIPTION(
390 This implements the multicopter rate controller. It takes rate setpoints (in acro mode 391 via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. 393 The controller has a PID loop for angular rate error. 397 PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
398 PRINT_MODULE_USAGE_COMMAND(
"start");
399 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
void setIntegratorLimit(const matrix::Vector3f &integrator_limit)
Set the mximum absolute value of the integrator for all axes.
uORB::Subscription _manual_control_sp_sub
manual control setpoint subscription
void vehicle_status_poll()
bool flag_control_rates_enabled
bool _actuators_0_circuit_breaker_enabled
circuit breaker to suppress output
uORB::Subscription _v_rates_sp_sub
vehicle rates setpoint subscription
measure the time elapsed performing an event
bool circuit_breaker_enabled_by_val(int32_t breaker_val, int32_t magic)
orb_advert_t _actuators_0_pub
attitude actuator controls publication
bool flag_control_rattitude_enabled
bool update(void *dst)
Copy the struct if updated.
uORB::Publication< vehicle_rates_setpoint_s > _v_rates_sp_pub
rate setpoint publication
uORB::Subscription _vehicle_land_detected_sub
vehicle land detected subscription
orb_id_t _actuators_id
pointer to correct actuator controls0 uORB metadata structure
__EXPORT int mc_rate_control_main(int argc, char *argv[])
Multicopter rate control app start / stop handling function.
static int custom_command(int argc, char *argv[])
int main(int argc, char **argv)
perf_counter_t _loop_perf
loop duration performance counter
float get_landing_gear_state()
Get the landing gear state based on the manual control switch position.
const T superexpo(const T &value, const T &e, const T &g)
Limiting / constrain helper functions.
landing_gear_s _landing_gear
uORB::Subscription _v_control_mode_sub
vehicle control mode subscription
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed)
Run one control loop cycle calculation.
High-resolution timer with callouts and timekeeping.
uORB::Subscription _parameter_update_sub
parameter updates subscription
collection of rather simple mathematical functions that get used over and over again ...
uORB::Subscription _vehicle_status_sub
vehicle status subscription
void setSaturationStatus(const MultirotorMixer::saturation_status &status)
Set saturation status.
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D)
Set the rate control gains.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
Set update frequency and low-pass filter cutoff that is applied to the derivative term...
uint16_t saturation_status
bool publish(const T &data)
Publish the struct.
bool flag_control_velocity_enabled
bool flag_control_manual_enabled
bool flag_control_termination_enabled
void perf_free(perf_counter_t handle)
Free a counter.
float _battery_status_scale
void resetIntegral()
Set the integral term to 0 to prevent windup.
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
float _thrust_sp
thrust setpoint
uORB::Subscription _landing_gear_sub
float _loop_update_rate_hz
current rate-controller loop update rate in [Hz]
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
Get status message of controller for logging/debugging.
uORB::PublicationMulti< rate_ctrl_status_s > _controller_status_pub
controller status publication
void perf_end(perf_counter_t handle)
End a performance event.
uORB::Subscription _battery_status_sub
battery status subscription
bool flag_control_attitude_enabled
uORB::Publication< landing_gear_s > _landing_gear_pub
bool updated()
Check if there is a new update.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
uORB::Subscription _motor_limits_sub
motor limits subscription
bool flag_control_altitude_enabled
void setFeedForwardGain(const matrix::Vector3f &FF)
Set direct rate to torque feed forward gain.
static int task_spawn(int argc, char *argv[])
Vector3< float > Vector3f
RateControl _rate_control
class for rate control calculations
#define CBRK_RATE_CTRL_KEY
bool flag_control_position_enabled
static int print_usage(const char *reason=nullptr)
uint64_t timestamp_sample
void parameters_updated()
initialize some vectors/matrices from parameters
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
virtual ~MulticopterRateControl()
void unregisterCallback()
manual_control_setpoint_s _manual_control_sp
bool _gear_state_initialized
true if the gear state has been initialized
bool update(void *dst)
Update the struct.
bool publish(const T &data)
Publish the struct.
matrix::Vector3f _rates_sp
angular rates setpoint
bool copy(void *dst)
Copy the struct.
int print_status() override
void perf_begin(perf_counter_t handle)
Begin a performance event.
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub
__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.
Matrix< Type, M, N > emult(const Matrix< Type, M, N > &other) const
vehicle_control_mode_s _v_control_mode
vehicle_status_s _vehicle_status