Compare commits

...

5 Commits

Author SHA1 Message Date
Silvan Fuhrer 76ea472696 Move Quadchute from the VTOL module to Commander (failure detector)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-03-28 13:49:05 +02:00
Silvan Fuhrer a803fa809b VTOL: remove some unsued variables
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-03-27 17:02:29 +02:00
Silvan Fuhrer 24f3476f00 vtol_vehicle_status: replace several status bools by single vehicle_vtol_state
Replace vtol_in_rw_mode, vtol_in_trans_mode, in_transition_to_fw by vehicle_vtol_state.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-03-27 17:01:49 +02:00
Silvan Fuhrer 77dbc05a89 Commander/VTOL: move VT_FW_PERM_STAB param to commander and not pass through as flag
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-03-27 17:00:07 +02:00
Silvan Fuhrer 23480dbdbe VTOL: move to cpp params API
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-03-27 16:57:20 +02:00
24 changed files with 527 additions and 804 deletions
@@ -31,6 +31,8 @@ param set-default BAT1_R_INTERNAL 0.0025
param set-default CBRK_AIRSPD_CHK 162128
param set-default CBRK_IO_SAFETY 22027
param set-default COM_FW_PERM_STAB 1
param set-default EKF2_GPS_POS_X -0.12
param set-default EKF2_IMU_POS_X -0.12
param set-default EKF2_TAU_VEL 0.5
@@ -131,7 +133,6 @@ param set-default VT_F_TRANS_DUR 1
param set-default VT_IDLE_PWM_MC 1025
param set-default VT_B_REV_OUT 0.5
param set-default VT_B_TRANS_THR 0.7
param set-default VT_FW_PERM_STAB 1
param set-default VT_TRANS_TIMEOUT 22
param set-default VT_F_TRANS_RAMP 4
+2 -2
View File
@@ -69,15 +69,15 @@ uint8 system_type # system type, contains mavlink MAV_TYPE
uint8 system_id # system id, contains MAVLink's system ID field
uint8 component_id # subsystem / component id, contains MAVLink's component ID field
uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground)
uint8 vehicle_type # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
# If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
# and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
bool is_vtol # True if the system is VTOL capable
bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW
bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode
bool in_transition_mode # True if VTOL is doing a transition
bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
bool vtol_fw_actuation_failure # True if an issue with fixed-wing actuation was detected
bool rc_signal_lost # true if RC reception lost
-1
View File
@@ -38,7 +38,6 @@ bool offboard_control_signal_lost
bool rc_signal_found_once
bool rc_calibration_in_progress
bool rc_calibration_valid # set if RC calibration is valid
bool vtol_transition_failure # Set to true if vtol transition failed
bool usb_connected # status of the USB power supply
bool sd_card_detected_once # set to true if the SD card was detected
+1 -5
View File
@@ -7,8 +7,4 @@ uint8 VEHICLE_VTOL_STATE_FW = 4
uint64 timestamp # time since system start (microseconds)
bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode
bool vtol_in_trans_mode
bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
bool vtol_transition_failsafe # vtol in transition failsafe mode
bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode
uint8 vehicle_vtol_state # current state of the vtol, see VEHICLE_VTOL_STATE
+8
View File
@@ -174,6 +174,14 @@ bool param_modify_on_import(bson_node_t node)
if (strcmp("IMU_GYRO_NF_BW", node->name) == 0) {
strcpy(node->name, "IMU_GYRO_NF0_BW");
PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_BW", "IMU_GYRO_NF0_BW");
}
}
// 2022-01-20: translate VT_FW_PERM_STAB to COM_FW_PERM_STAB
{
if (strcmp("VT_FW_PERM_STAB", node->name) == 0) {
strcpy(node->name, "COM_FW_PERM_STAB");
PX4_INFO("copying %s -> %s", "VT_FW_PERM_STAB", "COM_FW_PERM_STAB");
return true;
}
}
@@ -513,7 +513,8 @@ void AirspeedModule::update_wind_estimator_sideslip()
// update wind and airspeed estimator
_wind_estimator_sideslip.update(_time_now_usec);
if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
if (_vehicle_local_position_valid
&& _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q);
+61 -26
View File
@@ -817,7 +817,7 @@ Commander::Commander() :
_status_flags.rc_calibration_valid = true;
// default for vtol is rotary wing
_vtol_status.vtol_in_rw_mode = true;
_vtol_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
@@ -2041,8 +2041,10 @@ Commander::run()
if (!_armed.armed) {
_status.system_type = _param_mav_type.get();
const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status)
&& _vtol_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status)
&& _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
const bool is_ground = is_ground_rover(_status);
/* disable manual override for all systems that rely on electronic stabilization */
@@ -2232,35 +2234,33 @@ Commander::run()
if (_vtol_vehicle_status_sub.updated()) {
/* vtol status changed */
_vtol_vehicle_status_sub.copy(&_vtol_status);
_status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle really is of type vtol */
if (is_vtol(_status)) {
// Check if there has been any change while updating the flags
const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
// Check if there has been any change while updating the flags (transition = rotary wing status)
const auto new_vehicle_type = _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ?
vehicle_status_s::VEHICLE_TYPE_FIXED_WING :
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
if (new_vehicle_type != _status.vehicle_type) {
_status.vehicle_type = _vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
_status.vehicle_type = new_vehicle_type;
_status_changed = true;
}
if (_status.in_transition_mode != _vtol_status.vtol_in_trans_mode) {
_status.in_transition_mode = _vtol_status.vtol_in_trans_mode;
const bool new_in_transition = _vtol_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW
|| _vtol_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
if (_status.in_transition_mode != new_in_transition) {
_status.in_transition_mode = new_in_transition;
_status_changed = true;
}
if (_status.in_transition_to_fw != _vtol_status.in_transition_to_fw) {
_status.in_transition_to_fw = _vtol_status.in_transition_to_fw;
_status_changed = true;
}
if (_status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) {
_status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe;
if (_status.in_transition_to_fw != (_vtol_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW)) {
_status.in_transition_to_fw = (_vtol_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW);
_status_changed = true;
}
@@ -2768,7 +2768,32 @@ Commander::run()
}
}
if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
const bool flight_state_enable_quadchute = _status.is_vtol
&& (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _status.in_transition_mode);
if (flight_state_enable_quadchute && !_quadchute_triggered) {
if (fd_status_flags.qc_roll || fd_status_flags.qc_pitch || fd_status_flags.qc_min_alt || fd_status_flags.qc_alt_err
|| fd_status_flags.qc_trans_tmt) {
_quadchute_triggered = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: Transition to hover\t");
/* EVENT
* @description
* Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger.
*
* <profile name="dev">
* Quadchute can be disabled with the parameter <param>QC_enabled</param>.
* </profile>
*/
events::send(events::ID("commander_fd_quadchute"), {events::Log::Emergency, events::LogInternal::Warning},
"Critical failure detected: transition to hover");
// send_quadchute_command();
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, vehicle_status_s::VEHICLE_TYPE_ROTARY_WING,
1.0f);
_status.vtol_fw_actuation_failure = true;
_status_changed = true
}
} else if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
if (is_right_after_takeoff && !_lockdown_triggered) {
@@ -3449,11 +3474,9 @@ Commander::update_control_mode()
bool
Commander::stabilization_required()
{
return (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || // is a rotary wing, or
_status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or
(_vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND
_status.vehicle_type ==
vehicle_status_s::VEHICLE_TYPE_FIXED_WING)); // is a fixed wing, ie: transitioning back to rotary wing mode
// is a rotary-wing or fixed-wing/VTOL in fixed-wing mode and stabilistion is on
return (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ||
_param_com_fw_perm_stab.get());
}
void
@@ -4389,6 +4412,18 @@ void Commander::send_parachute_command()
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
}
// void Commander::send_quadchute_command()
// {
// uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
// vehicle_status_s vehicle_status{};
// vehicle_status_sub.copy(&vehicle_status);
// send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
// (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
// vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
// vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f);
// }
void Commander::checkWindAndWarn()
{
wind_s wind_estimate;
+3 -1
View File
@@ -228,6 +228,8 @@ private:
// Quadchute
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
(ParamBool<px4::params::COM_FW_PERM_STAB>) _param_com_fw_perm_stab,
// Offboard
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
@@ -322,7 +324,7 @@ private:
bool _flight_termination_triggered{false};
bool _lockdown_triggered{false};
bool _imbalanced_propeller_check_triggered{false};
bool _quadchute_triggered{false};
hrt_abstime _datalink_last_heartbeat_gcs{0};
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
+12
View File
@@ -1077,3 +1077,15 @@ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
* @unit m/s
*/
PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f);
/**
* Permanent stabilization in fixed-wing mode
*
* If set to true this parameter will cause permanent attitude stabilization
* for fixed-wing planes or VTOLs in fixed-wing mode.
* This parameter has been introduced for pure convenience sake.
*
* @boolean
* @group Commander
*/
PARAM_DEFINE_INT32(COM_FW_PERM_STAB, 0);
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -52,7 +52,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
failure_detector_status_u status_prev = _status;
if (vehicle_control_mode.flag_control_attitude_enabled) {
updateAttitudeStatus();
updateAttitudeStatus(vehicle_status);
if (_param_fd_ext_ats_en.get()) {
updateExternalAtsStatus();
@@ -73,10 +73,82 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
updateImbalancedPropStatus();
}
if (vehicle_status.is_vtol) {
updateMinHeightStatus(vehicle_status);
updateAdaptiveQC(vehicle_status, vehicle_control_mode);
updateTransitionTimeout();
}
return _status.value != status_prev.value;
}
void FailureDetector::updateAttitudeStatus()
void FailureDetector::updateMinHeightStatus(const vehicle_status_s &vehicle_status)
{
vehicle_local_position_s vehicle_local_position;
if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
if (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _param_vt_fw_min_alt.get() > 0
&& -(vehicle_local_position.z) < _param_vt_fw_min_alt.get()) {
_status.flags.qc_min_alt = true;
} else {
_status.flags.qc_min_alt = false;
}
}
}
void FailureDetector::updateAdaptiveQC(const vehicle_status_s &vehicle_status,
const vehicle_control_mode_s &vehicle_control_mode)
{
// adaptive quadchute, only enabled when TECS is running (so pure FW mode)
if (_param_vt_fw_alt_err.get() > 0 && vehicle_control_mode.flag_control_climb_rate_enabled
&& vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
tecs_status_s tecs_status;
if (_tecs_status_sub.update(&tecs_status)) {
// 1 second rolling average
_ra_hrate = (49 * _ra_hrate + tecs_status.height_rate) / 50;
_ra_hrate_sp = (49 * _ra_hrate_sp + tecs_status.height_rate_setpoint) / 50;
// are we dropping while requesting significant ascend?
if (((tecs_status.altitude_sp - tecs_status.altitude_filtered) > _param_vt_fw_alt_err.get()) &&
(_ra_hrate < -1.0f) && (_ra_hrate_sp > 1.0f)) {
_status.flags.qc_alt_err = true;
}
}
} else {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
}
void FailureDetector::updateTransitionTimeout()
{
vtol_vehicle_status_s vtol_vehicle_status;
_vtol_vehicle_status_sub.update(&vtol_vehicle_status);
const bool transitioning_to_FW = vtol_vehicle_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
if (!_was_in_transition_FW_prev && transitioning_to_FW) {
_transition_start_timestamp = hrt_absolute_time();
_was_in_transition_FW_prev = true;
}
if (transitioning_to_FW && _param_vt_trans_timeout.get() > FLT_EPSILON) {
if (hrt_elapsed_time(&_transition_start_timestamp) > _param_vt_trans_timeout.get() * 1_s) {
// transition timeout occured, abort transition
_status.flags.qc_trans_tmt = true;
}
}
}
void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
{
vehicle_attitude_s attitude;
@@ -105,6 +177,31 @@ void FailureDetector::updateAttitudeStatus()
// Update status
_status.flags.roll = _roll_failure_hysteresis.get_state();
_status.flags.pitch = _pitch_failure_hysteresis.get_state();
const bool enable_quadchte = vehicle_status.is_vtol
&& vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
if (enable_quadchte) {
// fixed-wing maximum roll angle
if (_param_vt_fw_qc_r.get() > 0) {
if (fabsf(roll) > fabsf(math::radians(_param_vt_fw_qc_r.get()))) {
_status.flags.qc_roll = true;
}
}
// fixed-wing maximum pitch angle
if (_param_vt_fw_qc_p.get() > 0) {
if (fabsf(pitch) > fabsf(math::radians(_param_vt_fw_qc_p.get()))) {
_status.flags.qc_pitch = true;
}
}
} else {
// reset if not in transition or FW
_status.flags.qc_roll = true;
_status.flags.qc_pitch = true;
}
}
}
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -52,11 +52,15 @@
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>
@@ -70,6 +74,11 @@ union failure_detector_status_u {
uint16_t high_wind : 1;
uint16_t battery : 1;
uint16_t imbalanced_prop : 1;
uint16_t qc_roll : 1;
uint16_t qc_pitch : 1;
uint16_t qc_min_alt : 1;
uint16_t qc_alt_err : 1;
uint16_t qc_trans_tmt : 1;
} flags;
uint16_t value {0};
};
@@ -87,10 +96,13 @@ public:
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
private:
void updateAttitudeStatus();
void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateImbalancedPropStatus();
void updateMinHeightStatus(const vehicle_status_s &vehicle_status);
void updateAdaptiveQC(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
void updateTransitionTimeout();
failure_detector_status_u _status{};
@@ -110,6 +122,17 @@ private:
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint});
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
float _ra_hrate{0.f};
float _ra_hrate_sp{0.f};
hrt_abstime _transition_start_timestamp{0};
bool _was_in_transition_FW_prev{false};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
@@ -118,6 +141,12 @@ private:
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
(ParamInt<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
(ParamInt<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout
)
};
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -156,3 +156,63 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
/**
* QuadChute minimum height threshold
*
* If the current height (relative to takeoff point) drops below this value in fixed-wing mode, a "QuadChute" is triggered,
* which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
* Only enabled for VTOL vehicles.
* Set to 0 to disable the check.
*
* @min 0
* @max 500
* @unit m
* @increment 1
* @group Failure Detector
*/
PARAM_DEFINE_INT32(VT_FW_MIN_ALT, 0);
/**
* QuadChute altitude error thershold
*
* If the current altitude is more than this amount below the setpoint, a "QuadChute" is triggered,
* which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
* Only enabled for VTOL vehicles.
* Set to 0 to disable the check.
*
* @min 0
* @max 200
* @unit m
* @increment 1
* @group Failure Detector
*/
PARAM_DEFINE_INT32(VT_FW_ALT_ERR, 0);
/**
* QuadChute pitch threshold
*
* If the current pitch angle estimate is above this value, a "QuadChute" is triggered,
* which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
* Set to 0 to disable the check.
*
* @min 0
* @max 180
* @unit deg
* @group Failure Detector
*/
PARAM_DEFINE_INT32(VT_FW_QC_P, 0);
/**
* QuadChute roll threshold
*
* If the current roll angle estimate is above this value, a "QuadChute" is triggered,
* which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
* Set to 0 to disable the check.
*
* @min 0
* @max 180
* @unit deg
* @group Failure Detector
*/
PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
@@ -592,7 +592,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
} else if (status.engine_failure) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags.vtol_transition_failure) {
} else if (status.vtol_fw_actuation_failure) {
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
@@ -646,7 +646,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
} else if (status.vtol_fw_actuation_failure) {
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
@@ -762,7 +762,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status_flags.vtol_transition_failure) {
} else if (status.vtol_fw_actuation_failure) {
set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
+27 -65
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -54,47 +54,20 @@ Standard::Standard(VtolAttitudeControl *attc) :
{
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_schedule.transition_start = 0;
_pusher_active = false;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
}
void
Standard::parameters_update()
{
float v;
/* duration of a forwards transition to fw mode */
param_get(_params_handles_standard.pusher_ramp_dt, &v);
_params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f);
/* MC ramp up during back transition to mc mode */
param_get(_params_handles_standard.back_trans_ramp, &v);
_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration);
_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
/* pitch setpoint offset */
param_get(_params_handles_standard.pitch_setpoint_offset, &v);
_params_standard.pitch_setpoint_offset = math::radians(v);
/* reverse output */
param_get(_params_handles_standard.reverse_output, &v);
_params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f);
/* reverse output */
param_get(_params_handles_standard.reverse_delay, &v);
_params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f);
VtolType::updateParams();
// make sure that pusher ramp in backtransition is smaller than back transition (max) duration
_param_vt_b_trans_ramp.set(math::min(_param_vt_b_trans_ramp.get(), _param_vt_b_trans_dur.get()));
}
void Standard::update_vtol_state()
@@ -107,17 +80,12 @@ void Standard::update_vtol_state()
float mc_weight = _mc_roll_weight;
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (_vtol_vehicle_status->vtol_transition_failsafe) {
if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) {
// Failsafe event, engage mc motors immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_pusher_throttle = 0.0f;
_reverse_output = 0.0f;
//reset failsafe when FW is no longer requested
if (!_attc->is_fixed_wing_requested()) {
_vtol_vehicle_status->vtol_transition_failsafe = false;
}
} else if (!_attc->is_fixed_wing_requested()) {
// the transition to fw mode switch is off
@@ -132,7 +100,7 @@ void Standard::update_vtol_state()
// Regular backtransition
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
_vtol_schedule.transition_start = hrt_absolute_time();
_reverse_output = _params_standard.reverse_output;
_reverse_output = _param_vt_b_rev_out.get();
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
// failsafe back to mc mode
@@ -148,13 +116,13 @@ void Standard::update_vtol_state()
if (_local_pos->v_xy_valid) {
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get();
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
}
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
@@ -179,14 +147,14 @@ void Standard::update_vtol_state()
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_params->airspeed_disabled;
&& !_param_fw_arsp_mode.get();
const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
bool transition_to_fw = false;
if (minimum_trans_time_elapsed) {
if (airspeed_triggers_transition) {
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();
} else {
transition_to_fw = true;
@@ -248,45 +216,40 @@ void Standard::update_transition_state()
}
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
if (_params_standard.pusher_ramp_dt <= 0.0f) {
if (_param_vt_psher_rmp_dt.get() <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params->front_trans_throttle;
_pusher_throttle = _param_vt_f_trans_thr.get();
} else if (_pusher_throttle <= _params->front_trans_throttle) {
} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
_pusher_throttle = _param_vt_f_trans_thr.get() * time_since_trans_start / _param_vt_psher_rmp_dt.get();
}
_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
if (_airspeed_trans_blend_margin > 0.0f &&
PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend &&
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
time_since_trans_start > getMinimumFrontTransitionTime()) {
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
}
// ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout) {
// transition timeout occured, abort transition
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
}
}
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
@@ -300,17 +263,16 @@ void Standard::update_transition_state()
_pusher_throttle = 0.0f;
if (time_since_trans_start >= _params_standard.reverse_delay) {
if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
// Handle throttle reversal for active breaking
float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
float thrscale = (time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
thrscale = math::constrain(thrscale, 0.0f, 1.0f);
_pusher_throttle = thrscale * _params->back_trans_throttle;
_pusher_throttle = thrscale * _param_vt_b_trans_thr.get();
}
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_ramp > FLT_EPSILON) {
mc_weight = time_since_trans_start / _params_standard.back_trans_ramp;
if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
}
set_all_motor_state(motor_state::ENABLED);
@@ -353,7 +315,7 @@ void Standard::fill_actuator_outputs()
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
const bool elevon_lock = (_params->elevons_mc_lock == 1);
const bool elevon_lock = (_param_vt_elev_mc_lock.get() == 1);
switch (_vtol_schedule.flight_mode) {
case vtol_mode::MC_MODE:
+9 -19
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,8 +46,6 @@
#ifndef STANDARD_H
#define STANDARD_H
#include "vtol_type.h"
#include <parameters/param.h>
#include <drivers/drv_hrt.h>
class Standard : public VtolType
{
@@ -67,22 +65,6 @@ public:
private:
struct {
float pusher_ramp_dt;
float back_trans_ramp;
float pitch_setpoint_offset;
float reverse_output;
float reverse_delay;
} _params_standard;
struct {
param_t pusher_ramp_dt;
param_t back_trans_ramp;
param_t pitch_setpoint_offset;
param_t reverse_output;
param_t reverse_delay;
} _params_handles_standard;
enum class vtol_mode {
MC_MODE = 0,
TRANSITION_TO_FW,
@@ -100,5 +82,13 @@ private:
float _airspeed_trans_blend_margin{0.0f};
void parameters_update() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::VT_PSHER_RMP_DT>) _param_vt_psher_rmp_dt,
(ParamFloat<px4::params::VT_B_TRANS_RAMP>) _param_vt_b_trans_ramp,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamFloat<px4::params::VT_B_REV_OUT>) _param_vt_b_rev_out,
(ParamFloat<px4::params::VT_B_REV_DEL>) _param_vt_b_rev_del
)
};
#endif
+19 -58
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -55,21 +55,14 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
}
void
Tailsitter::parameters_update()
{
float v;
VtolType::updateParams();
param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
_params_tailsitter.fw_pitch_sp_offset = math::radians(v);
}
void Tailsitter::update_vtol_state()
@@ -82,15 +75,10 @@ void Tailsitter::update_vtol_state()
float pitch = Eulerf(Quatf(_v_att->q)).theta();
if (_vtol_vehicle_status->vtol_transition_failsafe) {
if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) {
// Failsafe event, switch to MC mode immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
//reset failsafe when FW is no longer requested
if (!_attc->is_fixed_wing_requested()) {
_vtol_vehicle_status->vtol_transition_failsafe = false;
}
} else if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
@@ -111,7 +99,7 @@ void Tailsitter::update_vtol_state()
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached pitch angle to switch to MC mode
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
}
@@ -132,17 +120,14 @@ void Tailsitter::update_vtol_state()
case vtol_mode::TRANSITION_FRONT_P1: {
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_params->airspeed_disabled;
&& !_param_fw_arsp_mode.get() ;
bool transition_to_fw = false;
if (pitch <= PITCH_TRANSITION_FRONT_P1) {
if (airspeed_triggers_transition) {
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
} else {
transition_to_fw = true;
@@ -155,14 +140,6 @@ void Tailsitter::update_vtol_state()
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
}
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout) {
// transition timeout occured, abort transition
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
}
}
break;
}
@@ -177,24 +154,20 @@ void Tailsitter::update_vtol_state()
switch (_vtol_schedule.flight_mode) {
case vtol_mode::MC_MODE:
_vtol_mode = mode::ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case vtol_mode::FW_MODE:
_vtol_mode = mode::FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case vtol_mode::TRANSITION_FRONT_P1:
_vtol_mode = mode::TRANSITION_TO_FW;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
case vtol_mode::TRANSITION_BACK:
_vtol_mode = mode::TRANSITION_TO_MC;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
}
}
@@ -245,24 +218,16 @@ void Tailsitter::update_transition_state()
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
const float trans_pitch_rate = M_PI_2_F / _param_vt_f_trans_dur.get() ;
if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout) {
// transition timeout occured, abort transition
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
}
}
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
const float trans_pitch_rate = M_PI_2_F / _param_vt_b_trans_dur.get() ;
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
@@ -276,10 +241,6 @@ void Tailsitter::update_transition_state()
_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_v_att_sp->timestamp = hrt_absolute_time();
const Eulerf euler_sp(_q_trans_sp);
@@ -338,9 +299,9 @@ void Tailsitter::fill_actuator_outputs()
_thrust_setpoint_1->xyz[2] = 0.f;
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_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];
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
@@ -349,15 +310,15 @@ void Tailsitter::fill_actuator_outputs()
_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
if (_param_vt_fw_difthr_en.get()) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
}
} else {
_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] = 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];
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];
@@ -371,7 +332,7 @@ void Tailsitter::fill_actuator_outputs()
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
}
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
+5 -9
View File
@@ -62,15 +62,6 @@ public:
void waiting_on_tecs() override;
private:
struct {
float fw_pitch_sp_offset;
} _params_tailsitter{};
struct {
param_t fw_pitch_sp_offset;
} _params_handles_tailsitter{};
enum class vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
@@ -89,5 +80,10 @@ private:
void parameters_update() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
)
};
#endif
+37 -76
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -63,38 +63,12 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
_params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP");
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
}
void
Tiltrotor::parameters_update()
{
float v;
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
_params_tiltrotor.tilt_mc = v;
/* vtol tilt mechanism position in transition mode */
param_get(_params_handles_tiltrotor.tilt_transition, &v);
_params_tiltrotor.tilt_transition = v;
/* vtol tilt mechanism position in fw mode */
param_get(_params_handles_tiltrotor.tilt_fw, &v);
_params_tiltrotor.tilt_fw = v;
/* vtol tilt mechanism position during motor spinup */
param_get(_params_handles_tiltrotor.tilt_spinup, &v);
_params_tiltrotor.tilt_spinup = v;
/* vtol front transition phase 2 duration */
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
_params_tiltrotor.front_trans_dur_p2 = v;
VtolType::updateParams();
}
void Tiltrotor::update_vtol_state()
@@ -105,15 +79,10 @@ void Tiltrotor::update_vtol_state()
* forward completely. For the backtransition the motors simply rotate back.
*/
if (_vtol_vehicle_status->vtol_transition_failsafe) {
if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) {
// Failsafe event, switch to MC mode immediately
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
//reset failsafe when FW is no longer requested
if (!_attc->is_fixed_wing_requested()) {
_vtol_vehicle_status->vtol_transition_failsafe = false;
}
} else if (!_attc->is_fixed_wing_requested()) {
// plane is in multicopter mode
@@ -137,7 +106,7 @@ void Tiltrotor::update_vtol_state()
break;
case vtol_mode::TRANSITION_BACK:
const bool exit_backtransition_tilt_condition = _tilt_control <= (_params_tiltrotor.tilt_mc + 0.01f);
const bool exit_backtransition_tilt_condition = _tilt_control <= (_param_vt_tilt_mc.get() + 0.01f);
// speed exit condition: use ground if valid, otherwise airspeed
bool exit_backtransition_speed_condition = false;
@@ -145,14 +114,14 @@ void Tiltrotor::update_vtol_state()
if (_local_pos->v_xy_valid) {
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get() ;
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
}
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
@@ -178,16 +147,16 @@ void Tiltrotor::update_vtol_state()
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
&& !_params->airspeed_disabled;
&& !_param_fw_arsp_mode.get() ;
bool transition_to_p2 = false;
if (time_since_trans_start > getMinimumFrontTransitionTime()) {
if (airspeed_triggers_transition) {
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
} else {
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
time_since_trans_start > getOpenLoopFrontTransitionTime();
}
}
@@ -199,23 +168,15 @@ void Tiltrotor::update_vtol_state()
_vtol_schedule.transition_start = hrt_absolute_time();
}
// check front transition timeout
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout) {
// transition timeout occured, abort transition
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
}
}
break;
}
case vtol_mode::TRANSITION_FRONT_P2:
// if the rotors have been tilted completely we switch to fw mode
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
if (_tilt_control >= _param_vt_tilt_fw.get()) {
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
_tilt_control = _param_vt_tilt_fw.get();
}
break;
@@ -263,7 +224,7 @@ void Tiltrotor::update_mc_state()
// reset this timestamp while disarmed
if (!_v_control_mode->flag_armed) {
_last_timestamp_disarmed = hrt_absolute_time();
_tilt_motors_for_startup = _params_tiltrotor.tilt_spinup > 0.01f; // spinup phase only required if spinup tilt > 0
_tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0
} else if (_tilt_motors_for_startup) {
// leave motors tilted forward after arming to allow them to spin up easier
@@ -274,12 +235,12 @@ void Tiltrotor::update_mc_state()
if (_tilt_motors_for_startup) {
if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) {
_tilt_control = _params_tiltrotor.tilt_spinup;
_tilt_control = _param_vt_tilt_spinup.get();
} else {
// duration phase 2: begin to adapt tilt to multicopter tilt
float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup);
_tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get());
_tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
(_last_timestamp_disarmed + spin_up_duration_p1));
}
@@ -287,11 +248,11 @@ void Tiltrotor::update_mc_state()
} else {
// normal operation
_tilt_control = VtolType::pusher_assist() + _params_tiltrotor.tilt_mc;
_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
_mc_yaw_weight = 1.0f;
// do thrust compensation only for legacy (static) allocation
if (_params->ctrl_alloc != 1) {
if (!_param_sys_ctrl_alloc.get()) {
_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
}
}
@@ -307,7 +268,7 @@ void Tiltrotor::update_fw_state()
_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
// make sure motors are tilted forward
_tilt_control = _params_tiltrotor.tilt_fw;
_tilt_control = _param_vt_tilt_fw.get();
}
void Tiltrotor::update_transition_state()
@@ -338,10 +299,10 @@ void Tiltrotor::update_transition_state()
set_all_motor_state(motor_state::ENABLED);
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
const float ramped_up_tilt = _params_tiltrotor.tilt_mc +
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
time_since_trans_start / _params->front_trans_duration;
if (_tilt_control <= _param_vt_tilt_trans.get()) {
const float ramped_up_tilt = _param_vt_tilt_mc.get() +
fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
time_since_trans_start / _param_vt_f_trans_dur.get() ;
// only allow increasing tilt (tilt in hover can already be non-zero)
_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@@ -351,16 +312,16 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = 1.0f;
_mc_yaw_weight = 1.0f;
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) {
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
(_params->transition_airspeed - _params->airspeed_blend);
if (!_param_fw_arsp_mode.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) {
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
(_param_vt_arsp_trans.get() - _param_vt_arsp_blend.get());
_mc_roll_weight = weight;
_mc_yaw_weight = weight;
}
// without airspeed do timed weight changes
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
time_since_trans_start > getMinimumFrontTransitionTime()) {
_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
(getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
@@ -372,15 +333,15 @@ void Tiltrotor::update_transition_state()
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
_params_tiltrotor.front_trans_dur_p2, _params_tiltrotor.tilt_transition, _params_tiltrotor.tilt_fw);
_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
_mc_roll_weight = 0.0f;
_mc_yaw_weight = 0.0f;
// ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range)
int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) *
(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
@@ -404,7 +365,7 @@ void Tiltrotor::update_transition_state()
float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
progress = math::constrain(progress, 0.0f, 1.0f);
_tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress);
_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
}
_mc_yaw_weight = 1.0f;
@@ -510,9 +471,9 @@ void Tiltrotor::fill_actuator_outputs()
_thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
if (_param_vt_fw_difthr_en.get()) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
}
} else {
@@ -533,7 +494,7 @@ void Tiltrotor::fill_actuator_outputs()
// Fixed wing output
fw_out[4] = _tilt_control;
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_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;
+9 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -62,23 +62,6 @@ public:
void blendThrottleAfterFrontTransition(float scale) override;
private:
struct {
float tilt_mc; /**< actuator value corresponding to mc tilt */
float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */
float tilt_fw; /**< actuator value corresponding to fw tilt */
float tilt_spinup; /**< actuator value corresponding to spinup tilt */
float front_trans_dur_p2;
} _params_tiltrotor;
struct {
param_t tilt_mc;
param_t tilt_transition;
param_t tilt_fw;
param_t tilt_spinup;
param_t front_trans_dur_p2;
} _params_handles_tiltrotor;
enum class vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
@@ -111,5 +94,13 @@ private:
hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
bool _tilt_motors_for_startup{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::VT_TILT_MC>) _param_vt_tilt_mc,
(ParamFloat<px4::params::VT_TILT_TRANS>) _param_vt_tilt_trans,
(ParamFloat<px4::params::VT_TILT_FW>) _param_vt_tilt_fw,
(ParamFloat<px4::params::VT_TILT_SPINUP>) _param_vt_tilt_spinup,
(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur
)
};
#endif
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -55,70 +55,22 @@ using namespace matrix;
using namespace time_literals;
VtolAttitudeControl::VtolAttitudeControl() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
{
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
// start vtol in rotary wing mode
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_params.idle_pwm_mc = PWM_DEFAULT_MIN;
_params.vtol_motor_id = 0;
_params_handles.sys_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
_params.ctrl_alloc = 0;
param_get(_params_handles.sys_ctrl_alloc, &_params.ctrl_alloc);
if (_params.ctrl_alloc != 1) {
// these are not used with dynamic control allocation
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
}
_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
_params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
_params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
_params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
_params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
_params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
_params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
_params_handles.pitch_min_rad = param_find("VT_PTCH_MIN");
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
_params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2");
_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
/* fetch initial parameter values */
parameters_update();
if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TAILSITTER) {
_vtol_type = new Tailsitter(this);
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TILTROTOR) {
_vtol_type = new Tiltrotor(this);
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::STANDARD) {
_vtol_type = new Standard(this);
} else {
@@ -180,8 +132,6 @@ void VtolAttitudeControl::vehicle_cmd_poll()
while (_vehicle_cmd_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
@@ -189,10 +139,10 @@ void VtolAttitudeControl::vehicle_cmd_poll()
// deny transition from MC to FW in Takeoff, Land, RTL and Orbit
if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
(vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
(_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
@@ -217,167 +167,21 @@ void VtolAttitudeControl::vehicle_cmd_poll()
}
void
VtolAttitudeControl::quadchute(QuadchuteReason reason)
{
if (!_vtol_vehicle_status.vtol_transition_failsafe) {
switch (reason) {
case QuadchuteReason::TransitionTimeout:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: transition timeout\t");
events::send(events::ID("vtol_att_ctrl_quadchute_tout"), events::Log::Critical,
"Quadchute triggered, due to transition timeout");
break;
case QuadchuteReason::ExternalCommand:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: external command\t");
events::send(events::ID("vtol_att_ctrl_quadchute_ext_cmd"), events::Log::Critical,
"Quadchute triggered, due to external command");
break;
case QuadchuteReason::MinimumAltBreached:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: minimum altitude breached\t");
events::send(events::ID("vtol_att_ctrl_quadchute_min_alt"), events::Log::Critical,
"Quadchute triggered, due to minimum altitude breach");
break;
case QuadchuteReason::LossOfAlt:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t");
events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical,
"Quadchute triggered, due to loss of altitude");
break;
case QuadchuteReason::LargeAltError:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: large altitude error\t");
events::send(events::ID("vtol_att_ctrl_quadchute_alt_err"), events::Log::Critical,
"Quadchute triggered, due to large altitude error");
break;
case QuadchuteReason::MaximumPitchExceeded:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum pitch exceeded\t");
events::send(events::ID("vtol_att_ctrl_quadchute_max_pitch"), events::Log::Critical,
"Quadchute triggered, due to maximum pitch angle exceeded");
break;
case QuadchuteReason::MaximumRollExceeded:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum roll exceeded\t");
events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical,
"Quadchute triggered, due to maximum roll angle exceeded");
break;
}
_vtol_vehicle_status.vtol_transition_failsafe = true;
}
}
int
VtolAttitudeControl::parameters_update()
{
float v;
int32_t l;
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
if (_params.ctrl_alloc != 1) {
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
param_get(_params_handles.vt_mc_on_fmu, &l);
_params.vt_mc_on_fmu = l;
/* vtol motor count */
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
// update parameters from storage
updateParams();
}
/* vtol fw permanent stabilization */
param_get(_params_handles.vtol_fw_permanent_stab, &l);
_vtol_vehicle_status.fw_permanent_stab = (l == 1);
param_get(_params_handles.vtol_type, &l);
_params.vtol_type = l;
/* vtol lock elevons in multicopter */
param_get(_params_handles.elevons_mc_lock, &l);
_params.elevons_mc_lock = (l == 1);
/* minimum relative altitude for FW mode (QuadChute) */
param_get(_params_handles.fw_min_alt, &v);
_params.fw_min_alt = v;
/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
param_get(_params_handles.fw_alt_err, &v);
_params.fw_alt_err = v;
/* maximum pitch angle (QuadChute) */
param_get(_params_handles.fw_qc_max_pitch, &l);
_params.fw_qc_max_pitch = l;
/* maximum roll angle (QuadChute) */
param_get(_params_handles.fw_qc_max_roll, &l);
_params.fw_qc_max_roll = l;
param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);
/*
* Open loop transition time needs to be larger than minimum transition time,
* anything else makes no sense and can potentially lead to numerical problems.
*/
if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) {
_params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f;
param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time\t");
/* EVENT
* @description <param>VT_F_TR_OL_TM</param> set to {1:.1}.
*/
events::send<float>(events::ID("vtol_att_ctrl_ol_trans_too_large"), events::Log::Warning,
"Open loop transition time set larger than minimum transition time", _params.front_trans_time_openloop);
}
param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
param_get(_params_handles.airspeed_mode, &l);
_params.airspeed_disabled = l != 0;
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
param_get(_params_handles.diff_thrust, &_params.diff_thrust);
param_get(_params_handles.diff_thrust_scale, &v);
_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
/* maximum down pitch allowed */
param_get(_params_handles.pitch_min_rad, &v);
_params.pitch_min_rad = math::radians(v);
/* maximum down pitch allowed during landing*/
param_get(_params_handles.land_pitch_min_rad, &v);
_params.land_pitch_min_rad = math::radians(v);
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
param_get(_params_handles.back_trans_dec_sp, &v);
// increase the target deceleration setpoint provided to the controller by 20%
// to make overshooting the transition waypoint less likely in the presence of tracking errors
_params.back_trans_dec_sp = 1.2f * v;
param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
// update the parameters of the instances of base VtolType
if (_vtol_type != nullptr) {
_vtol_type->parameters_update();
}
return OK;
}
void
@@ -404,7 +208,6 @@ VtolAttitudeControl::Run()
_last_run_timestamp = now;
if (!_initialized) {
parameters_update(); // initialize parameter cache
if (_vtol_type->init()) {
_initialized = true;
@@ -439,15 +242,7 @@ VtolAttitudeControl::Run()
}
if (should_run) {
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
parameters_update();
}
parameters_update();
_v_control_mode_sub.update(&_v_control_mode);
_v_att_sub.update(&_v_att);
@@ -457,6 +252,7 @@ VtolAttitudeControl::Run()
_airspeed_validated_sub.update(&_airspeed_validated);
_tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected);
_vehicle_status_sub.update(&_vehicle_status);
action_request_poll();
vehicle_cmd_poll();
@@ -467,8 +263,8 @@ VtolAttitudeControl::Run()
}
// check if mc and fw sp were updated
bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
@@ -476,11 +272,21 @@ VtolAttitudeControl::Run()
// check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW:
// vehicle is doing a transition to FW
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
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);
}
break;
case mode::TRANSITION_TO_MC:
// vehicle is doing a transition
_vtol_vehicle_status.vtol_in_trans_mode = true;
_vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition
_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW);
// vehicle is doing a transition to MC
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
@@ -493,9 +299,7 @@ VtolAttitudeControl::Run()
case mode::ROTARY_WING:
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
@@ -504,9 +308,7 @@ VtolAttitudeControl::Run()
case mode::FIXED_WING:
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false;
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
if (fw_att_sp_updated) {
_vtol_type->update_fw_state();
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -53,12 +53,12 @@
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
@@ -92,20 +92,10 @@ using namespace time_literals;
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public ModuleParams, public px4::WorkItem
{
public:
enum class QuadchuteReason {
TransitionTimeout = 0,
ExternalCommand,
MinimumAltBreached,
LossOfAlt,
LargeAltError,
MaximumPitchExceeded,
MaximumRollExceeded,
};
VtolAttitudeControl();
~VtolAttitudeControl() override;
@@ -121,10 +111,8 @@ 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; }
@@ -144,14 +132,13 @@ public:
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_status_s *get_vehicle_status() {return &_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 Params *get_params() {return &_params;}
private:
void Run() override;
@@ -209,48 +196,10 @@ private:
vehicle_local_position_s _local_pos{};
vehicle_local_position_setpoint_s _local_pos_sp{};
vtol_vehicle_status_s _vtol_vehicle_status{};
vehicle_status_s _vehicle_status{};
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3]
Params _params{}; // struct holding the parameters
struct {
param_t idle_pwm_mc;
param_t vtol_motor_id;
param_t vtol_fw_permanent_stab;
param_t vtol_type;
param_t elevons_mc_lock;
param_t fw_min_alt;
param_t fw_alt_err;
param_t fw_qc_max_pitch;
param_t fw_qc_max_roll;
param_t front_trans_time_openloop;
param_t front_trans_time_min;
param_t front_trans_duration;
param_t back_trans_duration;
param_t transition_airspeed;
param_t front_trans_throttle;
param_t back_trans_throttle;
param_t airspeed_blend;
param_t airspeed_mode;
param_t front_trans_timeout;
param_t mpc_xy_cruise;
param_t fw_motors_off;
param_t diff_thrust;
param_t diff_thrust_scale;
param_t pitch_min_rad;
param_t land_pitch_min_rad;
param_t forward_thrust_scale;
param_t dec_to_pitch_ff;
param_t dec_to_pitch_i;
param_t back_trans_dec_sp;
param_t vt_mc_on_fmu;
param_t vt_forward_thrust_enable_mode;
param_t mpc_land_alt1;
param_t mpc_land_alt2;
param_t sys_ctrl_alloc;
} _params_handles{};
hrt_abstime _last_run_timestamp{0};
/* for multicopters it is usual to have a non-zero idle speed of the engines
@@ -268,5 +217,9 @@ private:
void action_request_poll();
void vehicle_cmd_poll();
int parameters_update(); //Update local parameter cache
void parameters_update();
DEFINE_PARAMETERS(
(ParamInt<px4::params::VT_TYPE>) _param_vt_type
)
};
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -51,17 +51,6 @@
*/
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
/**
* Permanent stabilization in fw mode
*
* If set to one this parameter will cause permanent attitude stabilization in fw mode.
* This parameter has been introduced for pure convenience sake.
*
* @boolean
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
*
@@ -218,50 +207,6 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);
*/
PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
/**
* QuadChute Altitude
*
* Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
* the vehicle will transition back to MC mode and enter failsafe RTL
* @min 0.0
* @max 200.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
/**
* Adaptive QuadChute
*
* Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint
* the vehicle will transition back to MC mode and enter failsafe RTL.
* @min 0.0
* @max 200.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
/**
* QuadChute Max Pitch
*
* Maximum pitch angle before QuadChute engages
* Above this the vehicle will transition back to MC mode and enter failsafe RTL
* @min 0
* @max 180
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_QC_P, 0);
/**
* QuadChute Max Roll
*
* Maximum roll angle before QuadChute engages
* Above this the vehicle will transition back to MC mode and enter failsafe RTL
* @min 0
* @max 180
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
/**
* Airspeed less front transition time (open loop)
*
+47 -111
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -52,6 +52,7 @@ using namespace matrix;
VtolType::VtolType(VtolAttitudeControl *att_controller) :
ModuleParams(nullptr),
_attc(att_controller),
_vtol_mode(mode::ROTARY_WING)
{
@@ -74,7 +75,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_airspeed_validated = _attc->get_airspeed();
_tecs_status = _attc->get_tecs_status();
_land_detected = _attc->get_land_detected();
_params = _attc->get_params();
_vehicle_status = _attc->get_vehicle_status();
for (auto &pwm_max : _max_mc_pwm_values.values) {
pwm_max = PWM_DEFAULT_MAX;
@@ -87,8 +88,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
bool VtolType::init()
{
if (_params->ctrl_alloc != 1) {
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
if (!_param_sys_ctrl_alloc.get()) {
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
@@ -124,8 +125,8 @@ bool VtolType::init()
px4_close(fd);
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id);
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off);
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_mot_id.get());
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_fw_mot_offid.get());
// in order to get the main motors we take all motors and clear the alternate motor bits
@@ -140,6 +141,16 @@ bool VtolType::init()
}
void VtolType::parameters_update()
{
updateParams();
// make sure that transition speed is above blending speed
_param_vt_arsp_trans.set(math::max(_param_vt_arsp_trans.get(), _param_vt_arsp_blend.get()));
// make sure that openloop transition time is above minimum time
_param_vt_f_tr_ol_tm.set(math::max(_param_vt_f_tr_ol_tm.get(), _param_vt_trans_min_tm.get()));
}
void VtolType::update_mc_state()
{
if (!_flag_idle_mc) {
@@ -203,8 +214,6 @@ void VtolType::update_fw_state()
blendThrottleAfterFrontTransition(progress);
}
}
check_quadchute_condition();
}
void VtolType::update_transition_state()
@@ -214,10 +223,6 @@ void VtolType::update_transition_state()
_transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f);
_last_loop_ts = t_now;
_throttle_blend_start_ts = t_now;
check_quadchute_condition();
}
float VtolType::update_and_get_backtransition_pitch_sp()
@@ -229,12 +234,16 @@ float VtolType::update_and_get_backtransition_pitch_sp()
const float track = atan2f(_local_pos->vy, _local_pos->vx);
const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
// increase the target deceleration setpoint provided to the controller by 20%
// to make overshooting the transition waypoint less likely in the presence of tracking errors
const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f;
// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
const float accel_error_forward = dec_sp + accel_body_forward;
const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
const float pitch_sp_new = _param_vt_b_dec_ff.get() * dec_sp + _accel_to_pitch_integ;
float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward;
if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
(pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) {
@@ -252,91 +261,17 @@ bool VtolType::can_transition_on_ground()
return !_v_control_mode->flag_armed || _land_detected->landed;
}
void VtolType::check_quadchute_condition()
{
if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
&& !_quadchute_command_treated) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand);
_quadchute_command_treated = true;
_attc->reset_immediate_transition();
} else {
_quadchute_command_treated = false;
}
if (!_tecs_running) {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
if (_v_control_mode->flag_armed && !_land_detected->landed) {
Eulerf euler = Quatf(_v_att->q);
// fixed-wing minimum altitude
if (_params->fw_min_alt > FLT_EPSILON) {
if (-(_local_pos->z) < _params->fw_min_alt) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
}
}
// adaptive quadchute
if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
// We use tecs for tracking in FW and local_pos_sp during transitions
if (_tecs_running) {
// 1 second rolling average
_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
// are we dropping while requesting significant ascend?
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) &&
(_ra_hrate < -1.0f) &&
(_ra_hrate_sp > 1.0f)) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt);
}
} else {
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err);
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
if (height_error && height_rate_error) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError);
}
}
}
// fixed-wing maximum pitch angle
if (_params->fw_qc_max_pitch > 0) {
if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
}
}
// fixed-wing maximum roll angle
if (_params->fw_qc_max_roll > 0) {
if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
}
}
}
}
bool VtolType::set_idle_mc()
{
if (_params->ctrl_alloc == 1) {
if (_param_sys_ctrl_alloc.get()) {
return true;
}
unsigned pwm_value = _params->idle_pwm_mc;
unsigned pwm_value = _param_vt_idle_pwm_mc.get();
struct pwm_output_values pwm_values {};
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
pwm_values.values[i] = pwm_value;
} else {
@@ -351,14 +286,14 @@ bool VtolType::set_idle_mc()
bool VtolType::set_idle_fw()
{
if (_params->ctrl_alloc == 1) {
if (_param_sys_ctrl_alloc.get()) {
return true;
}
struct pwm_output_values pwm_values {};
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
pwm_values.values[i] = PWM_DEFAULT_MIN;
} else {
@@ -373,7 +308,7 @@ bool VtolType::set_idle_fw()
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
{
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
@@ -404,7 +339,7 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
void VtolType::set_all_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
if (_param_sys_ctrl_alloc.get()) {
return;
}
@@ -414,7 +349,7 @@ void VtolType::set_all_motor_state(const motor_state target_state, const int val
void VtolType::set_main_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
if (_param_sys_ctrl_alloc.get()) {
return;
}
@@ -428,7 +363,7 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
{
if (_params->ctrl_alloc == 1) {
if (_param_sys_ctrl_alloc.get()) {
return;
}
@@ -465,7 +400,7 @@ bool VtolType::set_motor_state(const motor_state target_state, const int32_t cha
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = _params->idle_pwm_mc;
_current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get();
}
}
@@ -523,7 +458,7 @@ float VtolType::pusher_assist()
}
// disable pusher assist depending on setting of forward_thrust_enable_mode:
switch (_params->vt_forward_thrust_enable_mode) {
switch (_param_vt_fwd_thrust_en.get()) {
case DISABLE: // disable in all modes
return 0.0f;
break;
@@ -538,14 +473,14 @@ float VtolType::pusher_assist()
break;
case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) {
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get())) {
return 0.0f;
}
break;
case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) {
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get())) {
return 0.0f;
}
@@ -555,7 +490,7 @@ float VtolType::pusher_assist()
if ((_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
&& _v_control_mode->flag_control_auto_enabled) ||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) {
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) {
return 0.0f;
}
@@ -565,7 +500,7 @@ float VtolType::pusher_assist()
if ((_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
&& _v_control_mode->flag_control_auto_enabled) ||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) {
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) {
return 0.0f;
}
@@ -574,13 +509,13 @@ float VtolType::pusher_assist()
// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
// then the pusher-for-pitch strategy is disabled and we can return
if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
|| _v_control_mode->flag_control_altitude_enabled)) {
return 0.0f;
}
// Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive)
if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
if (_attc->get_vehicle_status()->vtol_fw_actuation_failure) {
return 0.0f;
}
@@ -605,11 +540,12 @@ float VtolType::pusher_assist()
// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
float forward_thrust = 0.0f;
float pitch_setpoint_min = _params->pitch_min_rad;
float pitch_setpoint_min = math::radians(_param_vt_ptch_min.get());
if (_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift)
pitch_setpoint_min = math::radians(
_param_vt_lnd_ptch_min.get()); // set min pitch during LAND (usually lower to generate less lift)
}
// only allow pitching down up to threshold, the rest of the desired
@@ -619,7 +555,7 @@ float VtolType::pusher_assist()
// desired roll angle in heading frame stays the same
const float roll_new = -asinf(body_z_sp(1));
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale;
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _param_vt_fwd_thrust_sc.get();
// limit forward actuation to [0, 0.9]
forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f);
@@ -670,10 +606,10 @@ float VtolType::getFrontTransitionTimeFactor() const
float VtolType::getMinimumFrontTransitionTime() const
{
return getFrontTransitionTimeFactor() * _params->front_trans_time_min;
return getFrontTransitionTimeFactor() * _param_vt_trans_min_tm.get();
}
float VtolType::getOpenLoopFrontTransitionTime() const
{
return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop;
return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get();
}
+38 -52
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -46,42 +46,7 @@
#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
struct Params {
int32_t ctrl_alloc;
int32_t idle_pwm_mc; // pwm value for idle in mc mode
int32_t vtol_motor_id;
int32_t vtol_type;
bool elevons_mc_lock; // lock elevons in multicopter mode
float fw_min_alt; // minimum relative altitude for FW mode (QuadChute)
float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute)
float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute)
float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute)
float front_trans_time_openloop;
float front_trans_time_min;
float front_trans_duration;
float back_trans_duration;
float transition_airspeed;
float front_trans_throttle;
float back_trans_throttle;
float airspeed_blend;
bool airspeed_disabled;
float front_trans_timeout;
float mpc_xy_cruise;
int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */
int32_t diff_thrust;
float diff_thrust_scale;
float pitch_min_rad;
float land_pitch_min_rad;
float forward_thrust_scale;
float dec_to_pitch_ff;
float dec_to_pitch_i;
float back_trans_dec_sp;
bool vt_mc_on_fmu;
int32_t vt_forward_thrust_enable_mode;
float mpc_land_alt1;
float mpc_land_alt2;
};
#include <px4_platform_common/module_params.h>
// Has to match 1:1 msg/vtol_vehicle_status.msg
enum class mode {
@@ -128,7 +93,7 @@ enum class pwm_limit_type {
class VtolAttitudeControl;
class VtolType
class VtolType: public ModuleParams
{
public:
@@ -174,11 +139,6 @@ public:
*/
virtual void waiting_on_tecs() {}
/**
* Checks for fixed-wing failsafe condition and issues abort request if needed.
*/
void check_quadchute_condition();
/**
* Returns true if we're allowed to do a mode transition on the ground.
*/
@@ -227,17 +187,15 @@ public:
struct airspeed_validated_s *_airspeed_validated; // airspeed
struct tecs_status_s *_tecs_status;
struct vehicle_land_detected_s *_land_detected;
struct vehicle_status_s *_vehicle_status;
struct vehicle_torque_setpoint_s *_torque_setpoint_0;
struct vehicle_torque_setpoint_s *_torque_setpoint_1;
struct vehicle_thrust_setpoint_s *_thrust_setpoint_0;
struct vehicle_thrust_setpoint_s *_thrust_setpoint_1;
struct Params *_params;
bool _flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
bool _pusher_active = false;
float _mc_roll_weight = 1.0f; // weight for multicopter attitude controller roll output
float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output
float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output
@@ -247,9 +205,6 @@ public:
float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only)
float _last_thr_in_fw_mode = 0.0f;
float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition
float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition
bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition
hrt_abstime _trans_finished_ts = 0;
@@ -265,9 +220,6 @@ public:
float _accel_to_pitch_integ = 0;
bool _quadchute_command_treated{false};
/**
* @brief Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures
* that they are spinning in mc mode.
@@ -292,6 +244,40 @@ public:
float update_and_get_backtransition_pitch_sp();
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
(ParamFloat<px4::params::VT_F_TR_OL_TM>) _param_vt_f_tr_ol_tm,
(ParamFloat<px4::params::VT_TRANS_MIN_TM>) _param_vt_trans_min_tm,
(ParamFloat<px4::params::VT_F_TRANS_DUR>) _param_vt_f_trans_dur,
(ParamFloat<px4::params::VT_B_TRANS_DUR>) _param_vt_b_trans_dur,
(ParamFloat<px4::params::VT_ARSP_TRANS>) _param_vt_arsp_trans,
(ParamFloat<px4::params::VT_F_TRANS_THR>) _param_vt_f_trans_thr,
(ParamFloat<px4::params::VT_B_TRANS_THR>) _param_vt_b_trans_thr,
(ParamFloat<px4::params::VT_ARSP_BLEND>) _param_vt_arsp_blend,
(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamBool<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
(ParamFloat<px4::params::VT_FW_DIFTHR_SC>) _param_vt_fw_difthr_sc,
(ParamFloat<px4::params::VT_B_DEC_FF>) _param_vt_b_dec_ff,
(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,
(ParamFloat<px4::params::VT_PTCH_MIN>) _param_vt_ptch_min,
(ParamFloat<px4::params::VT_FWD_THRUST_SC>) _param_vt_fwd_thrust_sc,
(ParamInt<px4::params::VT_FWD_THRUST_EN>) _param_vt_fwd_thrust_en,
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::VT_LND_PTCH_MIN>) _param_vt_lnd_ptch_min,
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _param_sys_ctrl_alloc,
(ParamInt<px4::params::VT_IDLE_PWM_MC>) _param_vt_idle_pwm_mc,
(ParamInt<px4::params::VT_MOT_ID>) _param_vt_mot_id,
(ParamBool<px4::params::VT_MC_ON_FMU>) _param_vt_mc_on_fmu,
(ParamInt<px4::params::VT_FW_MOT_OFFID>) _param_vt_fw_mot_offid
)
private: