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

VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics 

MC/FW rate controller and auto tuner: remove actuator_controls

AirshipAttControl: remove actuator_controls

MulticopterLandDetector: remove actuator_controls

mavlink streams vfr_hud and high_latency2: remove actuator_controls

RoverPositionController: remove actuator_controls

UUVAttitudeController: remove actuator_controls

battery: use length of thrust_setpoint for throttle compensation

VehicleMagnetometer: use length of thrust_setpoint for throttle compensation

Signed-off-by: Silvan Fuhrer
This commit is contained in:
Silvan Fuhrer 2022-12-23 12:57:18 +01:00 committed by Beat Küng
parent bcd6e7adee
commit b16f16598b
47 changed files with 332 additions and 549 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
msg/ActuatorControls.msg
View File

@ -1,19 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint8 NUM_ACTUATOR_CONTROLS = 9
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint8 INDEX_ROLL = 0
uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
uint8 INDEX_THROTTLE = 3
uint8 INDEX_GIMBAL_SHUTTER = 3
uint8 INDEX_CAMERA_ZOOM = 4
uint8 GROUP_INDEX_ATTITUDE = 0
uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
uint8 GROUP_INDEX_GIMBAL = 2
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[9] control
# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2
# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

7
msg/ActuatorControlsStatus.msg
View File

@ -1,10 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint8 INDEX_ROLL = 0
uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
uint8 INDEX_THROTTLE = 3
float32[4] control_power
float32[3] control_power
# TOPICS actuator_controls_status_0 actuator_controls_status_1

1
msg/CMakeLists.txt
View File

@ -39,7 +39,6 @@ include(px4_list_make_absolute)
set(msg_files
ActionRequest.msg
ActuatorArmed.msg
ActuatorControls.msg
ActuatorControlsStatus.msg
ActuatorMotors.msg
ActuatorOutputs.msg

3
msg/VehicleThrustSetpoint.msg
View File

@ -3,3 +3,6 @@ uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # thrust setpoint along X, Y, Z body axis [-1, 1]
# TOPICS vehicle_thrust_setpoint
# TOPICS vehicle_thrust_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_mc

3
msg/VehicleTorqueSetpoint.msg
View File

@ -3,3 +3,6 @@ uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized)
# TOPICS vehicle_torque_setpoint
# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc

1
src/drivers/px4io/px4io.cpp
View File

@ -68,7 +68,6 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command.h>

8
src/lib/battery/battery.cpp
View File

@ -213,9 +213,11 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
cell_voltage += _params.r_internal * current_a;
} else {
actuator_controls_s actuator_controls{};
_actuator_controls_0_sub.copy(&actuator_controls);
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
const float throttle = thrust_setpoint.length();
_throttle_filter.update(throttle);
if (!_battery_initialized) {

4
src/lib/battery/battery.h
View File

@ -55,9 +55,9 @@
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
/**
* BatteryBase is a base class for any type of battery.
@ -153,7 +153,7 @@ private:
void computeScale();
float computeRemainingTime(float current_a);
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};

1
src/lib/sensor_calibration/Magnetometer.hpp
View File

@ -38,7 +38,6 @@
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
namespace calibration

4
src/lib/system_identification/test_data.h
View File

@ -40,9 +40,9 @@
*
* Sampling frequency: ~285Hz, y_data is aligned with the closest next u_data
*
* u_data: actuator_controls_0.control[0] (roll control signal, used as the input)
* u_data: vehicle_torque_setpoint[0] (roll control signal, used as the input)
* y_data: vehicle_angular_velocity.xyz[0] (filtered roll angular rate, used as the output)
* t_data: actuator_controls_0.timestamp
* t_data: vehicle_torque_setpoint.timestamp
*/
#pragma once

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

@ -37,7 +37,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@ -77,8 +76,6 @@ private:
*/
void parameter_update_poll();
void publish_actuator_controls();
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
@ -88,13 +85,11 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
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)};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_status_s _vehicle_status{};
actuator_controls_s _actuator_controls{};
perf_counter_t _loop_perf;

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

@ -45,7 +45,6 @@ using namespace matrix;
AirshipAttitudeControl::AirshipAttitudeControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuator_controls_0_pub(ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
{
}
@ -80,36 +79,18 @@ AirshipAttitudeControl::parameter_update_poll()
}
}
void
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++) {
_actuator_controls.control[i] = 0.0f;
}
} else {
_actuator_controls.control[0] = 0.0f;
_actuator_controls.control[1] = _manual_control_setpoint.pitch;
_actuator_controls.control[2] = _manual_control_setpoint.yaw;
_actuator_controls.control[3] = (_manual_control_setpoint.throttle + 1.f) * .5f;
}
// note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run()
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls_0_pub.publish(_actuator_controls);
}
void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
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] = _actuator_controls.control[0];
v_torque_sp.xyz[1] = _actuator_controls.control[1];
v_torque_sp.xyz[2] = _actuator_controls.control[2];
// zero actuators if not armed
if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
v_torque_sp.xyz[0] = 0.f;
v_torque_sp.xyz[1] = _manual_control_setpoint.pitch;
v_torque_sp.xyz[2] = _manual_control_setpoint.yaw;
}
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
@ -119,9 +100,11 @@ 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] = _actuator_controls.control[3];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
// zero actuators if not armed
if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
v_thrust_sp.xyz[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
}
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
@ -142,10 +125,7 @@ AirshipAttitudeControl::Run()
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
_actuator_controls.timestamp_sample = angular_velocity.timestamp_sample;
/* run the rate controller immediately after a gyro update */
publish_actuator_controls();
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
@ -190,8 +170,6 @@ int AirshipAttitudeControl::print_status()
perf_print_counter(_loop_perf);
print_message(ORB_ID(actuator_controls_0), _actuator_controls);
return 0;
}

1
src/modules/commander/commander_helper.cpp
View File

@ -53,7 +53,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
View File

@ -226,7 +226,7 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
effectiveness(0 + 3, i + actuator_start_index) = 0.f;
effectiveness(1 + 3, i + actuator_start_index) = 0.f;
effectiveness(2 + 3, i + actuator_start_index) = ct;
effectiveness(2 + 3, i + actuator_start_index) = -ct;
}
}

20
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
View File

@ -44,7 +44,7 @@ using namespace matrix;
FwAutotuneAttitudeControl::FwAutotuneAttitudeControl(bool is_vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_actuator_controls_sub(this, is_vtol ? ORB_ID(actuator_controls_1) : ORB_ID(actuator_controls_0)),
_vehicle_torque_setpoint_sub(this, ORB_ID(vehicle_torque_setpoint), is_vtol ? 1 : 0),
_actuator_controls_status_sub(is_vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0))
{
_autotune_attitude_control_status_pub.advertise();
@ -65,7 +65,7 @@ bool FwAutotuneAttitudeControl::init()
_signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop
if (!_actuator_controls_sub.registerCallback()) {
if (!_vehicle_torque_setpoint_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
@ -81,7 +81,7 @@ void FwAutotuneAttitudeControl::Run()
{
if (should_exit()) {
_parameter_update_sub.unregisterCallback();
_actuator_controls_sub.unregisterCallback();
_vehicle_torque_setpoint_sub.unregisterCallback();
exit_and_cleanup();
return;
}
@ -101,7 +101,7 @@ void FwAutotuneAttitudeControl::Run()
// new control data needed every iteration
if ((_state == state::idle && !_aux_switch_en)
|| !_actuator_controls_sub.updated()) {
|| !_vehicle_torque_setpoint_sub.updated()) {
return;
}
@ -123,17 +123,17 @@ void FwAutotuneAttitudeControl::Run()
}
}
actuator_controls_s controls;
vehicle_torque_setpoint_s vehicle_torque_setpoint;
vehicle_angular_velocity_s angular_velocity;
if (!_actuator_controls_sub.copy(&controls)
if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)
|| !_vehicle_angular_velocity_sub.copy(&angular_velocity)) {
return;
}
perf_begin(_cycle_perf);
const hrt_abstime timestamp_sample = controls.timestamp;
const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp;
// collect sample interval average for filters
if (_last_run > 0) {
@ -152,15 +152,15 @@ void FwAutotuneAttitudeControl::Run()
checkFilters();
if (_state == state::roll) {
_sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL],
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[0],
angular_velocity.xyz[0]);
} else if (_state == state::pitch) {
_sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH],
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[1],
angular_velocity.xyz[1]);
} else if (_state == state::yaw) {
_sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_YAW],
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[2],
angular_velocity.xyz[2]);
}

4
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
View File

@ -52,13 +52,13 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <mathlib/mathlib.h>
#include <lib/systemlib/mavlink_log.h>
@ -108,7 +108,7 @@ private:
const matrix::Vector3f getIdentificationSignal();
uORB::SubscriptionCallbackWorkItem _actuator_controls_sub;
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub;
uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)};
uORB::Subscription _actuator_controls_status_sub;

114
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -43,8 +43,9 @@ using math::radians;
FixedwingRateControl::FixedwingRateControl(bool vtol) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_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)),
_vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)),
_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
/* fetch initial parameter values */
@ -123,24 +124,23 @@ FixedwingRateControl::vehicle_manual_poll()
if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// the controls must always be published in body (hover) frame
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
-(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get());
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
} else {
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
}
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() +
_param_trim_pitch.get(), -1.f, 1.f);
_vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f);
}
}
}
}
@ -350,31 +350,26 @@ void FixedwingRateControl::Run()
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
-1.f, 1.f) : trim_yaw;
_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw;
if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
_rate_control.resetIntegral();
}
/* throttle passed through if it is finite */
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
@ -384,7 +379,7 @@ void FixedwingRateControl::Run()
}
}
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
_vehicle_thrust_setpoint.xyz[0] *= _battery_scale;
}
}
@ -402,31 +397,28 @@ void FixedwingRateControl::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
_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);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_vehicle_torque_setpoint.xyz[2] + _param_fw_rll_to_yaw_ff.get() *
_vehicle_torque_setpoint.xyz[0], -1.f, 1.f);
// Tailsitter: rotate back to body frame from airspeed frame
if (_vehicle_status.is_vtol_tailsitter) {
const float helper = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_actuator_controls.control[actuator_controls_s::INDEX_YAW];
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper;
const float helper = _vehicle_torque_setpoint.xyz[0];
_vehicle_torque_setpoint.xyz[0] = _vehicle_torque_setpoint.xyz[2];
_vehicle_torque_setpoint.xyz[2] = -helper;
}
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = vehicle_angular_velocity.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) {
_actuator_controls_0_pub.publish(_actuator_controls);
{
_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
}
}
@ -479,44 +471,14 @@ void FixedwingRateControl::Run()
perf_end(_loop_perf);
}
void FixedwingRateControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
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] = _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);
}
void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
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] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingRateControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
float control_signal;
for (int i = 0; i < 3; i++) {
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 = _actuator_controls.control[i] - _control_prev[i];
_control_prev[i] = _actuator_controls.control[i];
} else {
control_signal = _actuator_controls.control[i];
}
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
const float control_signal = _vehicle_torque_setpoint.xyz[i] - _control_prev[i];
_control_prev[i] = _vehicle_torque_setpoint.xyz[i];
_control_energy[i] += control_signal * control_signal * dt;
}
@ -526,9 +488,9 @@ void FixedwingRateControl::updateActuatorControlsStatus(float dt)
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuator_controls.timestamp;
status.timestamp = _vehicle_torque_setpoint.timestamp;
for (int i = 0; i < 4; i++) {
for (int i = 0; i < 3; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}

12
src/modules/fw_rate_control/FixedwingRateControl.hpp
View File

@ -53,7 +53,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
@ -98,9 +97,6 @@ public:
private:
void Run() override;
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -117,18 +113,18 @@ private:
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
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<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub;
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{};
vehicle_torque_setpoint_s _vehicle_torque_setpoint{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};

10
src/modules/land_detector/MulticopterLandDetector.cpp
View File

@ -86,10 +86,10 @@ MulticopterLandDetector::MulticopterLandDetector()
void MulticopterLandDetector::_update_topics()
{
actuator_controls_s actuator_controls;
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
if (_actuator_controls_sub.update(&actuator_controls)) {
_actuator_controls_throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
_vehicle_thrust_setpoint_throttle = -vehicle_thrust_setpoint.xyz[2];
}
vehicle_control_mode_s vehicle_control_mode;
@ -207,7 +207,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available
const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f;
const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover;
_has_low_throttle = (_actuator_controls_throttle <= sys_low_throttle);
_has_low_throttle = (_vehicle_thrust_setpoint_throttle <= sys_low_throttle);
bool ground_contact = _has_low_throttle;
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
@ -256,7 +256,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
minimum_thrust_threshold = (_params.minManThrottle + 0.01f);
}
const bool minimum_thrust_now = _actuator_controls_throttle <= minimum_thrust_threshold;
const bool minimum_thrust_now = _vehicle_thrust_setpoint_throttle <= minimum_thrust_threshold;
_minimum_thrust_8s_hysteresis.set_state_and_update(minimum_thrust_now, now);
// Next look if vehicle is not rotating (do not consider yaw)

6
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -42,10 +42,10 @@
#pragma once
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/takeoff_status.h>
#include "LandDetector.h"
@ -105,7 +105,7 @@ private:
float crawlSpeed;
} _params{};
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
@ -118,7 +118,7 @@ private:
bool _flag_control_climb_rate_enabled{false};
bool _hover_thrust_initialized{false};
float _actuator_controls_throttle{0.f};
float _vehicle_thrust_setpoint_throttle{0.f};
uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};

13
src/modules/logger/logged_topics.cpp
View File

@ -47,9 +47,6 @@ void LoggedTopics::add_default_topics()
{
add_topic("action_request");
add_topic("actuator_armed");
add_topic("actuator_controls_0", 50);
add_topic("actuator_controls_1", 100);
add_topic("actuator_controls_2", 100);
add_optional_topic("actuator_controls_status_0", 300);
add_topic("airspeed", 1000);
add_optional_topic("airspeed_validated", 200);
@ -226,7 +223,7 @@ void LoggedTopics::add_default_topics()
// additional control allocation logging
add_topic("actuator_motors", 100);
add_topic("actuator_servos", 100);
add_topic_multi("vehicle_thrust_setpoint", 20, 1);
add_topic_multi("vehicle_thrust_setpoint", 20, 2);
add_topic_multi("vehicle_torque_setpoint", 20, 2);
// SYS_HITL: default ground truth logging for simulation
@ -241,10 +238,12 @@ void LoggedTopics::add_default_topics()
}
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw");
add_topic("actuator_controls_virtual_mc");
add_topic("fw_virtual_attitude_setpoint");
add_topic("mc_virtual_attitude_setpoint");
add_optional_topic("vehicle_torque_setpoint_virtual_mc");
add_optional_topic("vehicle_torque_setpoint_virtual_fw");
add_optional_topic("vehicle_thrust_setpoint_virtual_mc");
add_optional_topic("vehicle_thrust_setpoint_virtual_fw");
add_topic("time_offset");
add_topic("vehicle_angular_velocity", 10);
add_topic("vehicle_angular_velocity_groundtruth", 10);
@ -380,8 +379,6 @@ void LoggedTopics::add_raw_imu_accel_fifo()
void LoggedTopics::add_system_identification_topics()
{
// for system id need to log imu and controls at full rate
add_topic("actuator_controls_0");
add_topic("actuator_controls_1");
add_topic("sensor_combined");
add_topic("vehicle_angular_velocity");
add_topic("vehicle_torque_setpoint");

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -61,7 +61,6 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/autotune_attitude_control_status.h>

16
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -39,7 +39,6 @@
#include <lib/matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_selector_status.h>
@ -51,6 +50,7 @@
#include <uORB/topics/wind.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
@ -561,16 +561,16 @@ private:
if (_status_sub.update(&status)) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
actuator_controls_s actuator{};
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (_actuator_1_sub.copy(&actuator)) {
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
if (_vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint)) {
_throttle.add_value(vehicle_thrust_setpoint.xyz[0], _update_rate_filtered);
}
} else {
if (_actuator_0_sub.copy(&actuator)) {
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
if (_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint)) {
_throttle.add_value(-vehicle_thrust_setpoint.xyz[2], _update_rate_filtered);
}
}
@ -621,8 +621,8 @@ private:
msg.wp_num = UINT16_MAX;
}
uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)};
uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};

20
src/modules/mavlink/streams/VFR_HUD.hpp
View File

@ -35,10 +35,10 @@
#define VFR_HUD_HPP
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
class MavlinkStreamVFRHUD : public MavlinkStream
{
@ -65,8 +65,8 @@ private:
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _act0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _act1_sub{ORB_ID(actuator_controls_1)};
uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
@ -89,19 +89,19 @@ private:
msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading));
if (armed.armed) {
actuator_controls_s act0{};
actuator_controls_s act1{};
_act0_sub.copy(&act0);
_act1_sub.copy(&act1);
vehicle_thrust_setpoint_s vehicle_thrust_setpoint_0{};
vehicle_thrust_setpoint_s vehicle_thrust_setpoint_1{};
_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0);
_vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint_1);
// VFR_HUD throttle should only be used for operator feedback.
// VTOLs switch between actuator_controls_0 and actuator_controls_1. During transition there isn't a
// VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a
// a single throttle value, but this should still be a useful heuristic for operator awareness.
//
// Use ACTUATOR_CONTROL_TARGET if accurate states are needed.
msg.throttle = 100 * math::max(
act0.control[actuator_controls_s::INDEX_THROTTLE],
act1.control[actuator_controls_s::INDEX_THROTTLE]);
-vehicle_thrust_setpoint_0.xyz[2],
vehicle_thrust_setpoint_1.xyz[0]);
} else {
msg.throttle = 0.0f;

20
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
View File

@ -74,7 +74,7 @@ void McAutotuneAttitudeControl::Run()
{
if (should_exit()) {
_parameter_update_sub.unregisterCallback();
_actuator_controls_sub.unregisterCallback();
_vehicle_torque_setpoint_sub.unregisterCallback();
exit_and_cleanup();
return;
}
@ -92,7 +92,7 @@ void McAutotuneAttitudeControl::Run()
// new control data needed every iteration
if (_state == state::idle
|| !_actuator_controls_sub.updated()) {
|| !_vehicle_torque_setpoint_sub.updated()) {
return;
}
@ -112,17 +112,17 @@ void McAutotuneAttitudeControl::Run()
}
}
actuator_controls_s controls;
vehicle_torque_setpoint_s vehicle_torque_setpoint;
vehicle_angular_velocity_s angular_velocity;
if (!_actuator_controls_sub.copy(&controls)
if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)
|| !_vehicle_angular_velocity_sub.copy(&angular_velocity)) {
return;
}
perf_begin(_cycle_perf);
const hrt_abstime timestamp_sample = controls.timestamp;
const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp;
// collect sample interval average for filters
if (_last_run > 0) {
@ -142,15 +142,15 @@ void McAutotuneAttitudeControl::Run()
// Send data to the filters at maximum frequency
if (_state == state::roll) {
_sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL],
_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[0],
angular_velocity.xyz[0]);
} else if (_state == state::pitch) {
_sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH],
_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[1],
angular_velocity.xyz[1]);
} else if (_state == state::yaw) {
_sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_YAW],
_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[2],
angular_velocity.xyz[2]);
}
@ -498,7 +498,7 @@ void McAutotuneAttitudeControl::revertParamGains()
bool McAutotuneAttitudeControl::registerActuatorControlsCallback()
{
if (!_actuator_controls_sub.registerCallback()) {
if (!_vehicle_torque_setpoint_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
@ -581,7 +581,7 @@ void McAutotuneAttitudeControl::stopAutotune()
{
_param_mc_at_start.set(false);
_param_mc_at_start.commit();
_actuator_controls_sub.unregisterCallback();
_vehicle_torque_setpoint_sub.unregisterCallback();
}
const Vector3f McAutotuneAttitudeControl::getIdentificationSignal()

4
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
View File

@ -51,13 +51,13 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <mathlib/mathlib.h>
using namespace time_literals;
@ -102,7 +102,7 @@ private:
const matrix::Vector3f getIdentificationSignal();
uORB::SubscriptionCallbackWorkItem _actuator_controls_sub{this, ORB_ID(actuator_controls_0)};
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};
uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)};
uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)};

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

@ -46,7 +46,8 @@ using math::radians;
MulticopterRateControl::MulticopterRateControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)),
_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@ -221,21 +222,16 @@ MulticopterRateControl::Run()
rate_ctrl_status.timestamp = hrt_absolute_time();
_controller_status_pub.publish(rate_ctrl_status);
// publish actuator controls
actuator_controls_s actuators{};
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
2) : 0.0f;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// publish thrust and torque setpoints
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f;
vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f;
vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f;
// scale effort by battery status if enabled
// scale setpoints by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status;
@ -245,55 +241,35 @@ MulticopterRateControl::Run()
}
}
if (_battery_status_scale > 0.0f) {
for (int i = 0; i < 4; i++) {
actuators.control[i] *= _battery_status_scale;
if (_battery_status_scale > 0.f) {
for (int i = 0; i < 3; i++) {
vehicle_thrust_setpoint.xyz[i] = math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
vehicle_torque_setpoint.xyz[i] = math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
}
}
}
actuators.timestamp = hrt_absolute_time();
_actuator_controls_0_pub.publish(actuators);
vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
updateActuatorControlsStatus(actuators, dt);
vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
} 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();
_actuator_controls_0_pub.publish(actuators);
}
}
}
perf_end(_loop_perf);
}
void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
void MulticopterRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint,
float dt)
{
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
}
void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
}
void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)
{
for (int i = 0; i < 4; i++) {
_control_energy[i] += actuators.control[i] * actuators.control[i] * dt;
for (int i = 0; i < 3; i++) {
_control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt;
}
_energy_integration_time += dt;
@ -301,14 +277,14 @@ void MulticopterRateControl::updateActuatorControlsStatus(const actuator_control
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = actuators.timestamp;
status.timestamp = vehicle_torque_setpoint.timestamp;
for (int i = 0; i < 4; i++) {
for (int i = 0; i < 3; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}
_actuator_controls_status_0_pub.publish(status);
_actuator_controls_status_pub.publish(status);
_energy_integration_time = 0.f;
}
}

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

@ -46,7 +46,6 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
@ -88,10 +87,7 @@ private:
*/
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 hrt_abstime &timestamp_sample);
void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
RateControl _rate_control; ///< class for rate control calculations
@ -107,12 +103,11 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
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::Publication<actuator_controls_status_s> _actuator_controls_status_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> _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)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub;
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};

1
src/modules/navigator/mission_block.cpp
View File

@ -50,7 +50,6 @@
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vtol_vehicle_status.h>

1
src/modules/navigator/mission_block.h
View File

@ -47,7 +47,6 @@
#include <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command.h>

1
src/modules/payload_deliverer/payload_deliverer.h
View File

@ -48,7 +48,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;

1
src/modules/rc_update/rc_update.h
View File

@ -53,7 +53,6 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/input_rc.h>

40
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -149,13 +149,10 @@ RoverPositionControl::manual_control_setpoint_poll()
_attitude_sp_pub.publish(_att_sp);
} else {
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch;
// Set heading from the manual roll input channel
_act_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
_yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
// Set throttle from the manual throttle channel
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_throttle_control = (_manual_control_setpoint.throttle + 1.f) * .5f;
_reset_yaw_sp = true;
}
@ -276,21 +273,21 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
prev_wp(1));
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
_throttle_control = mission_throttle;
float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
_yaw_control = control_effort;
}
}
break;
case STOPPING: {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_yaw_control = 0.0f;
_throttle_control = 0.0f;
// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
(double)curr_wp(0), (double)curr_wp(1));
@ -336,7 +333,7 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
//Constrain maximum throttle to mission throttle
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, mission_throttle);
_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
Vector3f desired_body_velocity;
@ -352,12 +349,12 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
float control_effort = desired_theta / _param_max_turn_angle.get();
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
_yaw_control = control_effort;
} else {
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_throttle_control = 0.0f;
_yaw_control = 0.0f;
}
}
@ -371,11 +368,11 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
float control_effort = euler_sp(2) / _param_max_turn_angle.get();
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
_yaw_control = control_effort;
const float control_throttle = att_sp.thrust_body[0];
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f);
_throttle_control = math::constrain(control_throttle, 0.0f, 1.0f);
}
@ -481,22 +478,19 @@ RoverPositionControl::Run()
_control_mode.flag_control_attitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_manual_enabled) {
// timestamp and publish controls
_act_controls.timestamp = hrt_absolute_time();
_actuator_controls_pub.publish(_act_controls);
vehicle_thrust_setpoint_s v_thrust_sp{};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[0] = _throttle_control;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
vehicle_torque_setpoint_s v_torque_sp{};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW];
v_torque_sp.xyz[0] = 0.f;
v_torque_sp.xyz[1] = 0.f;
v_torque_sp.xyz[2] = _yaw_control;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
}
@ -541,7 +535,7 @@ int RoverPositionControl::print_usage(const char *reason)
### Description
Controls the position of a ground rover using an L1 controller.
Publishes `actuator_controls_0` messages at IMU_GYRO_RATEMAX.
Publishes `vehicle_thrust_setpoint (only in x) and vehicle_torque_setpoint (only yaw)` messages at IMU_GYRO_RATEMAX.
### Implementation
Currently, this implementation supports only a few modes:

5
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -72,7 +72,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
@ -108,7 +107,6 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
@ -125,7 +123,6 @@ private:
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
trajectory_setpoint_s _trajectory_setpoint{};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
@ -170,6 +167,8 @@ private:
float _manual_yaw_sp{0.0};
bool _reset_yaw_sp{true};
float _throttle_control{0.f};
float _yaw_control{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,

8
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -364,11 +364,13 @@ void VehicleMagnetometer::UpdatePowerCompensation()
if (_mag_comp_type != MagCompensationType::Disabled) {
// update power signal for mag compensation
if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) {
actuator_controls_s controls;
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) {
const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
if (_actuator_controls_0_sub.update(&controls)) {
for (auto &cal : _calibration) {
cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]);
cal.UpdatePower(thrust_setpoint.length());
}
}

4
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
View File

@ -50,7 +50,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/magnetometer_bias_estimate.h>
@ -60,6 +59,7 @@
#include <uORB/topics/sensors_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
using namespace time_literals;
@ -110,7 +110,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};

58
src/modules/uuv_att_control/uuv_att_control.cpp
View File

@ -95,34 +95,34 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
{
if (PX4_ISFINITE(roll_u)) {
roll_u = math::constrain(roll_u, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_ROLL] = roll_u;
_vehicle_torque_setpoint.xyz[0] = roll_u;
} else {
_actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f;
_vehicle_torque_setpoint.xyz[0] = 0.0f;
}
if (PX4_ISFINITE(pitch_u)) {
pitch_u = math::constrain(pitch_u, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_PITCH] = pitch_u;
_vehicle_torque_setpoint.xyz[1] = pitch_u;
} else {
_actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f;
_vehicle_torque_setpoint.xyz[1] = 0.0f;
}
if (PX4_ISFINITE(yaw_u)) {
yaw_u = math::constrain(yaw_u, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u;
_vehicle_torque_setpoint.xyz[2] = yaw_u;
} else {
_actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_vehicle_torque_setpoint.xyz[2] = 0.0f;
}
if (PX4_ISFINITE(thrust_x)) {
thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_x;
_vehicle_thrust_setpoint.xyz[0] = thrust_x;
} else {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_vehicle_thrust_setpoint.xyz[0] = 0.0f;
}
}
@ -265,48 +265,24 @@ void UUVAttitudeControl::Run()
_manual_control_setpoint.yaw,
_manual_control_setpoint.throttle, 0.f, 0.f);
}
}
_actuators.timestamp = hrt_absolute_time();
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_manual_enabled ||
_vcontrol_mode.flag_control_attitude_enabled) {
/* publish the actuator controls */
_actuator_controls_pub.publish(_actuators);
publishTorqueSetpoint(0);
publishThrustSetpoint(0);
_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint.timestamp_sample = 0.f;
_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint.timestamp_sample = 0.f;
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
}
perf_end(_loop_perf);
}
void UUVAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
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];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void UUVAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
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[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
int UUVAttitudeControl::task_spawn(int argc, char *argv[])
{
UUVAttitudeControl *instance = new UUVAttitudeControl();
@ -347,7 +323,7 @@ int UUVAttitudeControl::print_usage(const char *reason)
### Description
Controls the attitude of an unmanned underwater vehicle (UUV).
Publishes `actuator_controls_0` messages at a constant 250Hz.
Publishes `vehicle_thrust_setpont` and `vehicle_torque_setpoint` messages at a constant 250Hz.
### Implementation
Currently, this implementation supports only a few modes:

5
src/modules/uuv_att_control/uuv_att_control.hpp
View File

@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/uORB.h>
@ -103,7 +102,6 @@ private:
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};
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)};
@ -117,7 +115,8 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
actuator_controls_s _actuators{};
vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{};
vehicle_torque_setpoint_s _vehicle_torque_setpoint{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _attitude_setpoint{};
vehicle_rates_setpoint_s _rates_setpoint{};

2
src/modules/uuv_pos_control/uuv_pos_control.cpp
View File

@ -242,7 +242,7 @@ int UUVPOSControl::print_usage(const char *reason)
R"DESCR_STR(
### Description
Controls the attitude of an unmanned underwater vehicle (UUV).
Publishes `actuator_controls_0` messages at a constant 250Hz.
Publishes `attitude_setpoint` messages.
### Implementation
Currently, this implementation supports only a few modes:
* Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators

1
src/modules/uuv_pos_control/uuv_pos_control.hpp
View File

@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/uORB.h>
using matrix::Eulerf;

114
src/modules/vtol_att_control/standard.cpp
View File

@ -273,91 +273,75 @@ void Standard::update_fw_state()
*/
void Standard::fill_actuator_outputs()
{
auto &mc_in = _actuators_mc_in->control;
auto &fw_in = _actuators_fw_in->control;
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample;
_torque_setpoint_0->xyz[0] = 0.f;
_torque_setpoint_0->xyz[1] = 0.f;
_torque_setpoint_0->xyz[2] = 0.f;
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample;
_torque_setpoint_1->xyz[0] = 0.f;
_torque_setpoint_1->xyz[1] = 0.f;
_torque_setpoint_1->xyz[2] = 0.f;
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = 0.f;
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
// MC out = MC in
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL];
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW];
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
// MC actuators:
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0];
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1];
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2];
_thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2];
// FW out = 0, other than roll and pitch depending on elevon lock
fw_out[actuator_controls_s::INDEX_ROLL] = _param_vt_elev_mc_lock.get() ? 0 :
fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = _param_vt_elev_mc_lock.get() ? 0 :
fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = 0;
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
// FW actuators:
if (!_param_vt_elev_mc_lock.get()) {
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
}
_thrust_setpoint_0->xyz[0] = _pusher_throttle;
break;
case vtol_mode::TRANSITION_TO_FW:
// FALLTHROUGH
case vtol_mode::TRANSITION_TO_MC:
// MC out = MC in (weighted)
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
// MC actuators:
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2] * _mc_yaw_weight;
_thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight;
// FW out = FW in, with VTOL transition controlling throttle and airbrakes
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL] * (1.f - _mc_roll_weight);
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH] * (1.f - _mc_pitch_weight);
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW] * (1.f - _mc_yaw_weight);
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
// FW actuators
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * (1.f - _mc_roll_weight);
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * (1.f - _mc_pitch_weight);
_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * (1.f - _mc_yaw_weight);
_thrust_setpoint_0->xyz[0] = _pusher_throttle;
break;
case vtol_mode::FW_MODE:
// MC out = 0
mc_out[actuator_controls_s::INDEX_ROLL] = 0;
mc_out[actuator_controls_s::INDEX_PITCH] = 0;
mc_out[actuator_controls_s::INDEX_YAW] = 0;
mc_out[actuator_controls_s::INDEX_THROTTLE] = 0;
// FW out = FW in
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// FW actuators
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2];
_thrust_setpoint_0->xyz[0] = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
break;
}
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_torque_setpoint_0->xyz[0] = mc_out[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW];
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
}
void

66
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -257,92 +257,62 @@ void Tailsitter::update_fw_state()
*/
void Tailsitter::fill_actuator_outputs()
{
auto &mc_in = _actuators_mc_in->control;
auto &fw_in = _actuators_fw_in->control;
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample;
_torque_setpoint_0->xyz[0] = 0.f;
_torque_setpoint_0->xyz[1] = 0.f;
_torque_setpoint_0->xyz[2] = 0.f;
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample;
_torque_setpoint_1->xyz[0] = 0.f;
_torque_setpoint_1->xyz[1] = 0.f;
_torque_setpoint_1->xyz[2] = 0.f;
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = 0.f;
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL];
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW];
// Motors
if (_vtol_mode == vtol_mode::FW_MODE) {
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0];
/* allow differential thrust if enabled */
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
float yaw_control = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get();
mc_out[actuator_controls_s::INDEX_ROLL] = yaw_control;
_torque_setpoint_0->xyz[0] = yaw_control;
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get();
}
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
float pitch_control = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get();
mc_out[actuator_controls_s::INDEX_PITCH] = pitch_control;
_torque_setpoint_0->xyz[1] = pitch_control;
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * _param_vt_fw_difthr_s_p.get();
}
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
float roll_control = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get();
mc_out[actuator_controls_s::INDEX_YAW] = roll_control;
_torque_setpoint_0->xyz[2] = roll_control;
_torque_setpoint_0->xyz[2] = -_vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_r.get();
}
} else {
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW];
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0];
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1];
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2];
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2];
}
if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
} else {
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
// Control surfaces
if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) {
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2];
}
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
}

61
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -374,92 +374,67 @@ void Tiltrotor::waiting_on_tecs()
_v_att_sp->thrust_body[0] = _thrust_transition;
}
/**
* Write data to actuator output topic.
*/
void Tiltrotor::fill_actuator_outputs()
{
auto &mc_in = _actuators_mc_in->control;
auto &fw_in = _actuators_fw_in->control;
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample;
_torque_setpoint_0->xyz[0] = 0.f;
_torque_setpoint_0->xyz[1] = 0.f;
_torque_setpoint_0->xyz[2] = 0.f;
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->timestamp_sample;
_torque_setpoint_1->xyz[0] = 0.f;
_torque_setpoint_1->xyz[1] = 0.f;
_torque_setpoint_1->xyz[2] = 0.f;
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = 0.f;
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
// Multirotor output
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2] * _mc_yaw_weight;
// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
// pass the vector magnitude and collective tilt separately. MC also needs collective thrust on z.
// Passing 3D thrust plus tilt is not feasible as they
// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
// by the allocator directly.
float collective_thrust_normalized_setpoint = 0.f;
if (_vtol_mode == vtol_mode::FW_MODE) {
_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
collective_thrust_normalized_setpoint = fw_in[actuator_controls_s::INDEX_THROTTLE];
collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
/* allow differential thrust if enabled */
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ;
}
} else {
_thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight;
_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
}
// Fixed wing output
if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 0;
} else {
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) {
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2];
}
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
// publish tiltrotor extra controls
tiltrotor_extra_controls_s tiltrotor_extra_controls = {};
tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control;

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

@ -94,12 +94,22 @@ VtolAttitudeControl::~VtolAttitudeControl()
bool
VtolAttitudeControl::init()
{
if (!_actuator_inputs_mc.registerCallback()) {
if (!_vehicle_torque_setpoint_virtual_fw_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
if (!_actuator_inputs_fw.registerCallback()) {
if (!_vehicle_torque_setpoint_virtual_mc_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
if (!_vehicle_thrust_setpoint_virtual_fw_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
if (!_vehicle_thrust_setpoint_virtual_mc_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
}
@ -270,8 +280,10 @@ void
VtolAttitudeControl::Run()
{
if (should_exit()) {
_actuator_inputs_fw.unregisterCallback();
_actuator_inputs_mc.unregisterCallback();
_vehicle_torque_setpoint_virtual_fw_sub.unregisterCallback();
_vehicle_torque_setpoint_virtual_mc_sub.unregisterCallback();
_vehicle_thrust_setpoint_virtual_fw_sub.unregisterCallback();
_vehicle_thrust_setpoint_virtual_mc_sub.unregisterCallback();
exit_and_cleanup();
return;
}
@ -305,8 +317,10 @@ VtolAttitudeControl::Run()
perf_begin(_loop_perf);
const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in);
const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in);
const bool updated_fw_in = _vehicle_torque_setpoint_virtual_fw_sub.update(&_vehicle_torque_setpoint_virtual_fw)
|| _vehicle_thrust_setpoint_virtual_fw_sub.update(&_vehicle_thrust_setpoint_virtual_fw);
const bool updated_mc_in = _vehicle_torque_setpoint_virtual_mc_sub.update(&_vehicle_torque_setpoint_virtual_mc)
|| _vehicle_thrust_setpoint_virtual_mc_sub.update(&_vehicle_thrust_setpoint_virtual_mc);
// run on actuator publications corresponding to VTOL mode
bool should_run = false;
@ -414,8 +428,6 @@ VtolAttitudeControl::Run()
}
_vtol_type->fill_actuator_outputs();
_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);

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

@ -65,7 +65,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
@ -125,10 +124,11 @@ public:
float getAirDensity() const { return _air_density; }
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_mc() {return &_vehicle_torque_setpoint_virtual_mc;}
struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_fw() {return &_vehicle_torque_setpoint_virtual_fw;}
struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_mc() {return &_vehicle_thrust_setpoint_virtual_mc;}
struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_fw() {return &_vehicle_thrust_setpoint_virtual_fw;}
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;}
@ -149,9 +149,10 @@ public:
private:
void Run() override;
uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)};
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_fw)};
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_torque_setpoint_virtual_mc)};
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_fw_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_fw)};
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_virtual_mc_sub{this, ORB_ID(vehicle_thrust_setpoint_virtual_mc)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -171,8 +172,6 @@ private:
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(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<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
@ -188,10 +187,10 @@ private:
vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint
vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint
actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control
actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control
actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
vehicle_torque_setpoint_s _vehicle_torque_setpoint_virtual_mc{};
vehicle_torque_setpoint_s _vehicle_torque_setpoint_virtual_fw{};
vehicle_thrust_setpoint_s _vehicle_thrust_setpoint_virtual_mc{};
vehicle_thrust_setpoint_s _vehicle_thrust_setpoint_virtual_fw{};
vehicle_torque_setpoint_s _torque_setpoint_0{};
vehicle_torque_setpoint_s _torque_setpoint_1{};

10
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -62,10 +62,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
_v_control_mode = _attc->get_control_mode();
_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
_actuators_out_0 = _attc->get_actuators_out0();
_actuators_out_1 = _attc->get_actuators_out1();
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_vehicle_torque_setpoint_virtual_mc = _attc->get_vehicle_torque_setpoint_virtual_mc();
_vehicle_torque_setpoint_virtual_fw = _attc->get_vehicle_torque_setpoint_virtual_fw();
_vehicle_thrust_setpoint_virtual_mc = _attc->get_vehicle_thrust_setpoint_virtual_mc();
_vehicle_thrust_setpoint_virtual_fw = _attc->get_vehicle_thrust_setpoint_virtual_fw();
_torque_setpoint_0 = _attc->get_torque_setpoint_0();
_torque_setpoint_1 = _attc->get_torque_setpoint_1();
_thrust_setpoint_0 = _attc->get_thrust_setpoint_0();
@ -108,7 +108,7 @@ void VtolType::update_mc_state()
void VtolType::update_fw_state()
{
resetAccelToPitchPitchIntegrator();
_last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_last_thr_in_fw_mode = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));

8
src/modules/vtol_att_control/vtol_type.h
View File

@ -259,10 +259,10 @@ protected:
struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_rate_control
struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control
struct vehicle_torque_setpoint_s *_vehicle_torque_setpoint_virtual_mc;
struct vehicle_torque_setpoint_s *_vehicle_torque_setpoint_virtual_fw;
struct vehicle_thrust_setpoint_s *_vehicle_thrust_setpoint_virtual_mc;
struct vehicle_thrust_setpoint_s *_vehicle_thrust_setpoint_virtual_fw;
struct vehicle_local_position_s *_local_pos;
struct vehicle_local_position_setpoint_s *_local_pos_sp;
struct airspeed_validated_s *_airspeed_validated; // airspeed