diff --git a/src/modules/airship_att_control/airship_att_control.hpp b/src/modules/airship_att_control/airship_att_control.hpp index 857f773bf9..dbb876ec62 100644 --- a/src/modules/airship_att_control/airship_att_control.hpp +++ b/src/modules/airship_att_control/airship_att_control.hpp @@ -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, 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 ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_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 _actuators_0_pub; - uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _actuator_controls_0_pub; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _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 */ }; diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index af68d069af..e1b7d9972f 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -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 ×tamp_sample) @@ -107,9 +107,9 @@ void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_ 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 ×tamp_ 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); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index cb41426a24..94b01f2706 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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 ×tam 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 ×tam 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; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index a76111c949..117155ba69 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -106,24 +106,24 @@ private: void publishTorqueSetpoint(const hrt_abstime ×tamp_sample); void publishThrustSetpoint(const hrt_abstime ×tamp_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_sub{ORB_ID(airspeed_validated)}; - uORB::Publication _actuators_0_pub; + uORB::Publication _actuator_controls_0_pub; uORB::Publication _actuator_controls_status_pub; uORB::Publication _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; @@ -131,15 +131,15 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _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}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ac1166b17b..8aea8b6e14 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -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, 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 _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ - uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _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 _man_x_input_filter; AlphaFilter _man_y_input_filter; @@ -139,27 +135,25 @@ private: uint8_t _quat_reset_counter{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_mc_roll_p, - (ParamFloat) _param_mc_pitch_p, - (ParamFloat) _param_mc_yaw_p, - (ParamFloat) _param_mc_yaw_weight, + (ParamInt) _param_mc_airmode, + (ParamFloat) _param_mc_man_tilt_tau, - (ParamFloat) _param_mc_rollrate_max, + (ParamFloat) _param_mc_roll_p, + (ParamFloat) _param_mc_pitch_p, + (ParamFloat) _param_mc_yaw_p, + (ParamFloat) _param_mc_yaw_weight, + + (ParamFloat) _param_mc_rollrate_max, (ParamFloat) _param_mc_pitchrate_max, - (ParamFloat) _param_mc_yawrate_max, - - (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + (ParamFloat) _param_mc_yawrate_max, /* Stabilized mode params */ - (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ - (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ - (ParamFloat) - _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */ - (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ - - (ParamInt) _param_mc_airmode, - (ParamFloat) _param_mc_man_tilt_tau + (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ + (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ + (ParamFloat) _param_mpc_thr_hover, /**< throttle at stationary hover */ + (ParamInt) _param_mpc_thr_curve /**< throttle curve behavior */ ) }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3b9c7f87ac..88eb51aa51 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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); } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index c4bb0a7824..e19a8b0e1e 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -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); } } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 2e0523c660..ce02967658 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -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 ×tamp_sample); + void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime ×tamp_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 _actuators_0_pub; + uORB::Publication _actuator_controls_0_pub; uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _v_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _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) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, @@ -170,16 +172,13 @@ private: (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, (ParamFloat) _param_mc_acro_y_max, - (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ - (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ - (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ (ParamBool) _param_mc_bat_scale_en, (ParamInt) _param_cbrk_rate_ctrl ) - - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ - }; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 8d7e589dcd..244cbe2652 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -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 ×tamp_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, diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 45805de73f..47b9bf98a0 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -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{}; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index ea85555c60..819034df16 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 77a5393c65..aa41a9f1d0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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 _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) - uORB::Publication _actuators_1_pub{ORB_ID(actuator_controls_1)}; - uORB::Publication _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; - uORB::Publication _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; + uORB::Publication _actuator_controls_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) + uORB::Publication _actuator_controls_1_pub{ORB_ID(actuator_controls_1)}; + uORB::Publication _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::PublicationMulti _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)}; uORB::PublicationMulti _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::PublicationMulti _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)}; uORB::PublicationMulti _vehicle_torque_setpoint1_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _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();