dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Standardize variable naming and formatting across vehicle attitude controller files.

This commit is contained in:
mcsauder 2022-05-18 21:06:04 -06:00 committed by Daniel Agar
parent 422be90140
commit 6b0788ff46
12 changed files with 240 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/airship_att_control/airship_att_control.hpp
View File

@ -46,11 +46,6 @@
using namespace time_literals;
/**
* Airship attitude control app start / stop handling function
*/
extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
public px4::WorkItem
{
@ -80,27 +75,27 @@ private:
/**
* Check for parameter update and handle it.
*/
void parameter_update_poll();
void parameter_update_poll();
void publish_actuator_controls();
void publish_actuator_controls();
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
struct actuator_controls_s _actuators {}; /**< actuator controls */
manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
actuator_controls_s _actuator_controls {}; /**< actuator controls */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf; /**< loop performance counter */
};

35
src/modules/airship_att_control/airship_att_control_main.cpp
View File

@ -45,7 +45,7 @@ using namespace matrix;
AirshipAttitudeControl::AirshipAttitudeControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuators_0_pub(ORB_ID(actuator_controls_0)),
_actuator_controls_0_pub(ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
{
}
@ -86,20 +86,20 @@ AirshipAttitudeControl::publish_actuator_controls()
// zero actuators if not armed
if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
for (uint8_t i = 0 ; i < 4 ; i++) {
_actuators.control[i] = 0.0f;
_actuator_controls.control[i] = 0.0f;
}
} else {
_actuators.control[0] = 0.0f;
_actuators.control[1] = _manual_control_sp.x;
_actuators.control[2] = _manual_control_sp.r;
_actuators.control[3] = _manual_control_sp.z;
_actuator_controls.control[0] = 0.0f;
_actuator_controls.control[1] = _manual_control_sp.x;
_actuator_controls.control[2] = _manual_control_sp.r;
_actuator_controls.control[3] = _manual_control_sp.z;
}
// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
_actuators.timestamp = hrt_absolute_time();
// note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run()
_actuator_controls.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(_actuators);
_actuator_controls_0_pub.publish(_actuator_controls);
}
void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
@ -107,9 +107,9 @@ void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuators.control[0];
v_torque_sp.xyz[1] = _actuators.control[1];
v_torque_sp.xyz[2] = _actuators.control[2];
v_torque_sp.xyz[0] = _actuator_controls.control[0];
v_torque_sp.xyz[1] = _actuator_controls.control[1];
v_torque_sp.xyz[2] = _actuator_controls.control[2];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
@ -119,7 +119,7 @@ void AirshipAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestamp_
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuators.control[3];
v_thrust_sp.xyz[0] = _actuator_controls.control[3];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
@ -142,7 +142,7 @@ AirshipAttitudeControl::Run()
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
_actuator_controls.timestamp_sample = angular_velocity.timestamp_sample;
/* run the rate controller immediately after a gyro update */
publish_actuator_controls();
@ -190,7 +190,7 @@ int AirshipAttitudeControl::print_status()
perf_print_counter(_loop_perf);
print_message(ORB_ID(actuator_controls), _actuators);
print_message(ORB_ID(actuator_controls), _actuator_controls);
return 0;
}
@ -227,7 +227,10 @@ To reduce control latency, the module directly polls on the gyro topic published
return 0;
}
int airship_att_control_main(int argc, char *argv[])
/**
* Airship attitude control app start / stop handling function
*/
extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[])
{
return AirshipAttitudeControl::main(argc, argv);
}

74
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -41,7 +41,7 @@ using math::radians;
FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
@ -134,7 +134,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
@ -173,13 +173,13 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
} else {
/* manual/direct control */
_actuators.control[actuator_controls_s::INDEX_ROLL] =
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuators.control[actuator_controls_s::INDEX_PITCH] =
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuators.control[actuator_controls_s::INDEX_YAW] =
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
1.0f);
}
}
@ -536,14 +536,16 @@ void FixedwingAttitudeControl::Run()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
@ -558,11 +560,11 @@ void FixedwingAttitudeControl::Run()
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
/* add in manual rudder control in manual modes */
if (_vcontrol_mode.flag_control_manual_enabled) {
_actuators.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
}
if (!PX4_ISFINITE(yaw_u)) {
@ -571,13 +573,12 @@ void FixedwingAttitudeControl::Run()
}
/* throttle passed through if it is finite */
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])) ?
_att_sp.thrust_body[0] :
0.0f;
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
(PX4_ISFINITE(_att_sp.thrust_body[0])) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
@ -587,7 +588,7 @@ void FixedwingAttitudeControl::Run()
}
}
_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
}
}
@ -611,15 +612,16 @@ void FixedwingAttitudeControl::Run()
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch :
trim_pitch;
float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
_rates_sp.thrust_body[0] : 0.0f;
}
@ -640,24 +642,24 @@ void FixedwingAttitudeControl::Run()
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
_actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
_actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = att.timestamp;
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = att.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
_actuators_0_pub.publish(_actuators);
_actuator_controls_0_pub.publish(_actuator_controls);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
@ -676,9 +678,9 @@ void FixedwingAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestam
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators.control[actuator_controls_s::INDEX_YAW];
v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
@ -688,7 +690,7 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestam
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuators.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
@ -770,11 +772,11 @@ void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
if (i <= actuator_controls_status_s::INDEX_YAW) {
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
control_signal = _actuators.control[i] - _control_prev[i];
_control_prev[i] = _actuators.control[i];
control_signal = _actuator_controls.control[i] - _control_prev[i];
_control_prev[i] = _actuator_controls.control[i];
} else {
control_signal = _actuators.control[i];
control_signal = _actuator_controls.control[i];
}
_control_energy[i] += control_signal * control_signal * dt;
@ -785,7 +787,7 @@ void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuators.timestamp;
status.timestamp = _actuator_controls.timestamp;
for (int i = 0; i < 4; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;

36
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -106,24 +106,24 @@ private:
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
@ -131,15 +131,15 @@ private:
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
vehicle_local_position_s _local_pos {}; /**< local position */
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
actuator_controls_s _actuator_controls {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
vehicle_local_position_s _local_pos {}; /**< local position */
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _last_run{0};

72
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -60,11 +60,6 @@
using namespace time_literals;
/**
* Multicopter attitude control app start / stop handling function
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, public ModuleParams,
public px4::WorkItem
{
@ -89,40 +84,41 @@ private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void parameters_updated();
float throttle_curve(float throttle_stick_input);
float throttle_curve(float throttle_stick_input);
/**
* Generate & publish an attitude setpoint from stick inputs
*/
void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp);
void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp);
AttitudeControl _attitude_control; ///< class for attitude control calculations
AttitudeControl _attitude_control; /**< class for attitude control calculations */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */
vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */
perf_counter_t _loop_perf; /**< loop duration performance counter */
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
AlphaFilter<float> _man_x_input_filter;
AlphaFilter<float> _man_y_input_filter;
@ -139,27 +135,25 @@ private:
uint8_t _quat_reset_counter{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
(ParamFloat<px4::params::MC_YAW_WEIGHT>) _param_mc_yaw_weight,
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau,
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
(ParamFloat<px4::params::MC_YAW_WEIGHT>) _param_mc_yaw_weight,
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
/* Stabilized mode params */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>)
_param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, /**< throttle at stationary hover */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve /**< throttle curve behavior */
)
};

32
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -277,7 +277,7 @@ MulticopterAttitudeControl::Run()
/* check for updates in other topics */
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_v_control_mode_sub.update(&_v_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
@ -306,15 +306,15 @@ MulticopterAttitudeControl::Run()
// vehicle is a tailsitter in transition mode
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
if (run_att_ctrl) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
!_v_control_mode.flag_control_altitude_enabled &&
!_v_control_mode.flag_control_velocity_enabled &&
!_v_control_mode.flag_control_position_enabled) {
if (_vehicle_control_mode.flag_control_manual_enabled &&
!_vehicle_control_mode.flag_control_altitude_enabled &&
!_vehicle_control_mode.flag_control_velocity_enabled &&
!_vehicle_control_mode.flag_control_position_enabled) {
generate_attitude_setpoint(q, dt, _reset_yaw_sp);
attitude_setpoint_generated = true;
@ -340,14 +340,14 @@ MulticopterAttitudeControl::Run()
}
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};
v_rates_sp.roll = rates_sp(0);
v_rates_sp.pitch = rates_sp(1);
v_rates_sp.yaw = rates_sp(2);
_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
v_rates_sp.timestamp = hrt_absolute_time();
vehicle_rates_setpoint_s rates_setpoint{};
rates_setpoint.roll = rates_sp(0);
rates_setpoint.pitch = rates_sp(1);
rates_setpoint.yaw = rates_sp(2);
_thrust_setpoint_body.copyTo(rates_setpoint.thrust_body);
rates_setpoint.timestamp = hrt_absolute_time();
_v_rates_sp_pub.publish(v_rates_sp);
_vehicle_rates_setpoint_pub.publish(rates_setpoint);
}
// reset yaw setpoint during transitions, tailsitter.cpp generates
@ -425,7 +425,11 @@ https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth
return 0;
}
int mc_att_control_main(int argc, char *argv[])
/**
* Multicopter attitude control app start / stop handling function
*/
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
{
return MulticopterAttitudeControl::main(argc, argv);
}

53
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -46,7 +46,7 @@ using math::radians;
MulticopterRateControl::MulticopterRateControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@ -138,7 +138,7 @@ MulticopterRateControl::Run()
const Vector3f rates{angular_velocity.xyz};
/* check for updates in other topics */
_v_control_mode_sub.update(&_v_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
@ -169,7 +169,10 @@ MulticopterRateControl::Run()
}
}
if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) {
// use rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint{};
if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) {
// generate the rate setpoint from sticks
manual_control_setpoint_s manual_control_setpoint;
@ -184,37 +187,31 @@ MulticopterRateControl::Run()
_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_setpoint{};
v_rates_setpoint.roll = _rates_setpoint(0);
v_rates_setpoint.pitch = _rates_setpoint(1);
v_rates_setpoint.yaw = _rates_setpoint(2);
v_rates_setpoint.thrust_body[0] = 0.0f;
v_rates_setpoint.thrust_body[1] = 0.0f;
v_rates_setpoint.thrust_body[2] = _thrust_setpoint;
v_rates_setpoint.timestamp = hrt_absolute_time();
vehicle_rates_setpoint.roll = _rates_setpoint(0);
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
vehicle_rates_setpoint.thrust_body[0] = 0.0f;
vehicle_rates_setpoint.thrust_body[1] = 0.0f;
vehicle_rates_setpoint.thrust_body[2] = _thrust_setpoint;
vehicle_rates_setpoint.timestamp = hrt_absolute_time();
_v_rates_setpoint_pub.publish(v_rates_setpoint);
_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
}
} else {
// use rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint;
if (_v_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
if (_v_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
}
} else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
}
}
// run the rate controller
if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
if (_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
// reset integral if disarmed
if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_rate_control.resetIntegral();
}
@ -281,16 +278,16 @@ MulticopterRateControl::Run()
}
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
_actuator_controls_0_pub.publish(actuators);
updateActuatorControlsStatus(actuators, dt);
} else if (_v_control_mode.flag_control_termination_enabled) {
} else if (_vehicle_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
// publish actuator controls
actuator_controls_s actuators{};
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
_actuator_controls_0_pub.publish(actuators);
}
}
}

37
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -89,60 +89,62 @@ private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void parameters_updated();
void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _v_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
orb_advert_t _mavlink_log_pub{nullptr};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
bool _landed{true};
bool _maybe_landed{true};
float _battery_status_scale{0.0f};
hrt_abstime _last_run{0};
perf_counter_t _loop_perf; /**< loop duration performance counter */
// keep setpoint values between updates
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
matrix::Vector3f _rates_setpoint{};
float _battery_status_scale{0.0f};
float _thrust_setpoint{0.0f};
hrt_abstime _last_run{0};
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
float _energy_integration_time{0.0f};
float _control_energy[4] {};
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
@ -170,16 +172,13 @@ private:
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
)
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
};

45
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -90,7 +90,7 @@ void VehicleAngularVelocity::Stop()
{
// clear all registered callbacks
_sensor_sub.unregisterCallback();
_sensor_fifo_sub.unregisterCallback();
_sensor_gyro_fifo_sub.unregisterCallback();
_sensor_selection_sub.unregisterCallback();
Deinit();
@ -136,7 +136,7 @@ bool VehicleAngularVelocity::UpdateSampleRate()
const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
if (_fifo_available) {
_sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH));
_sensor_gyro_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH));
} else {
_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH));
@ -148,7 +148,7 @@ bool VehicleAngularVelocity::UpdateSampleRate()
} else {
_sensor_sub.set_required_updates(1);
_sensor_fifo_sub.set_required_updates(1);
_sensor_gyro_fifo_sub.set_required_updates(1);
_publish_interval_min_us = 0;
}
}
@ -279,7 +279,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
}
if (sensor_gyro_fifo_sub.get().device_id == device_id) {
if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) {
if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
// make sure non-FIFO sub is unregistered
_sensor_sub.unregisterCallback();
@ -323,7 +323,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u
if (sensor_gyro_sub.get().device_id == device_id) {
if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
// make sure FIFO sub is unregistered
_sensor_fifo_sub.unregisterCallback();
_sensor_gyro_fifo_sub.unregisterCallback();
_calibration.set_device_id(sensor_gyro_sub.get().device_id);
@ -788,7 +788,7 @@ void VehicleAngularVelocity::Run()
// process all outstanding fifo messages
sensor_gyro_fifo_s sensor_fifo_data;
while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
const int N = sensor_fifo_data.samples;
static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
@ -813,9 +813,10 @@ void VehicleAngularVelocity::Run()
}
// Publish
if (!_sensor_fifo_sub.updated()) {
if (!_sensor_gyro_fifo_sub.updated()) {
if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample,
angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
angular_velocity_uncalibrated,
angular_acceleration_uncalibrated)) {
perf_end(_cycle_perf);
return;
@ -856,7 +857,8 @@ void VehicleAngularVelocity::Run()
// Publish
if (!_sensor_sub.updated()) {
if (CalibrateAndPublish(sensor_data.timestamp_sample,
angular_velocity_uncalibrated, angular_acceleration_uncalibrated)) {
angular_velocity_uncalibrated,
angular_acceleration_uncalibrated)) {
perf_end(_cycle_perf);
return;
@ -875,33 +877,32 @@ void VehicleAngularVelocity::Run()
}
bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
const Vector3f &angular_velocity_uncalibrated, const Vector3f &angular_acceleration_uncalibrated)
const Vector3f &angular_velocity_uncalibrated,
const Vector3f &angular_acceleration_uncalibrated)
{
if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
// Publish vehicle_angular_acceleration
vehicle_angular_acceleration_s v_angular_acceleration;
v_angular_acceleration.timestamp_sample = timestamp_sample;
vehicle_angular_acceleration_s angular_acceleration;
angular_acceleration.timestamp_sample = timestamp_sample;
// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
_angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated;
_angular_acceleration.copyTo(v_angular_acceleration.xyz);
v_angular_acceleration.timestamp = hrt_absolute_time();
_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
_angular_acceleration.copyTo(angular_acceleration.xyz);
angular_acceleration.timestamp = hrt_absolute_time();
_vehicle_angular_acceleration_pub.publish(angular_acceleration);
// Publish vehicle_angular_velocity
vehicle_angular_velocity_s v_angular_velocity;
v_angular_velocity.timestamp_sample = timestamp_sample;
vehicle_angular_velocity_s angular_velocity;
angular_velocity.timestamp_sample = timestamp_sample;
// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
_angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias;
_angular_velocity.copyTo(v_angular_velocity.xyz);
v_angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(v_angular_velocity);
_angular_velocity.copyTo(angular_velocity.xyz);
angular_velocity.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(angular_velocity);
// shift last publish time forward, but don't let it get further behind than the interval
_last_publish = math::constrain(_last_publish + _publish_interval_min_us,

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -113,7 +113,7 @@ private:
uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
calibration::Gyroscope _calibration{};

16
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -301,8 +301,8 @@ VtolAttitudeControl::Run()
if (should_run) {
parameters_update();
_v_control_mode_sub.update(&_v_control_mode);
_v_att_sub.update(&_v_att);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
@ -333,7 +333,7 @@ VtolAttitudeControl::Run()
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_transition_state();
_v_att_sp_pub.publish(_v_att_sp);
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}
break;
@ -344,7 +344,7 @@ VtolAttitudeControl::Run()
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_transition_state();
_v_att_sp_pub.publish(_v_att_sp);
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}
break;
@ -355,7 +355,7 @@ VtolAttitudeControl::Run()
if (mc_att_sp_updated) {
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}
break;
@ -366,15 +366,15 @@ VtolAttitudeControl::Run()
if (fw_att_sp_updated) {
_vtol_type->update_fw_state();
_v_att_sp_pub.publish(_v_att_sp);
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}
break;
}
_vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1);
_actuator_controls_0_pub.publish(_actuators_out_0);
_actuator_controls_1_pub.publish(_actuators_out_1);
_vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0);
_vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1);

50
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -123,9 +123,13 @@ public:
bool init();
bool is_fixed_wing_requested() { return _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; };
void quadchute(QuadchuteReason reason);
int get_transition_command() {return _transition_command;}
bool get_immediate_transition() {return _immediate_transition;}
void reset_immediate_transition() {_immediate_transition = false;}
float getAirDensity() const { return _air_density; }
@ -137,20 +141,19 @@ public:
struct airspeed_validated_s *get_airspeed() {return &_airspeed_validated;}
struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;}
struct tecs_status_s *get_tecs_status() {return &_tecs_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_attitude_s *get_att() {return &_vehicle_attitude;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_vehicle_attitude_sp;}
struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vehicle_control_mode_s *get_control_mode() {return &_vehicle_control_mode;}
struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
struct vehicle_torque_setpoint_s *get_torque_setpoint_0() {return &_torque_setpoint_0;}
struct vehicle_torque_setpoint_s *get_torque_setpoint_1() {return &_torque_setpoint_1;}
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
private:
void Run() override;
@ -161,32 +164,32 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed subscription
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<actuator_controls_s> _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
uORB::Publication<actuator_controls_s> _actuators_1_pub{ORB_ID(actuator_controls_1)};
uORB::Publication<vehicle_attitude_setpoint_s> _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vtol_vehicle_status_s> _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust)
uORB::Publication<actuator_controls_s> _actuator_controls_1_pub{ORB_ID(actuator_controls_1)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _vehicle_torque_setpoint1_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<vtol_vehicle_status_s> _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)};
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint
vehicle_attitude_setpoint_s _vehicle_attitude_sp{}; // vehicle attitude setpoint
vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint
vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint
@ -200,11 +203,11 @@ private:
vehicle_thrust_setpoint_s _thrust_setpoint_0{};
vehicle_thrust_setpoint_s _thrust_setpoint_1{};
airspeed_validated_s _airspeed_validated{}; // airspeed
airspeed_validated_s _airspeed_validated{};
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude
vehicle_control_mode_s _v_control_mode{}; //vehicle control mode
vehicle_attitude_s _vehicle_attitude{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_land_detected_s _land_detected{};
vehicle_local_position_s _local_pos{};
vehicle_local_position_setpoint_s _local_pos_sp{};
@ -214,7 +217,7 @@ private:
hrt_abstime _last_run_timestamp{0};
/* for multicopters it is usual to have a non-zero idle speed of the engines
/* For multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC};
@ -224,9 +227,10 @@ private:
bool _initialized{false};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _loop_perf; // loop performance counter
void action_request_poll();
void vehicle_cmd_poll();
void parameters_update();