fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete unused PWM IOCTLs

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent bcdd2203d3
commit ce337a3d80
35 changed files with 198 additions and 1331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/esc_calibration.cpp
+3 -110
View File
@@ -91,8 +91,10 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, f
}
}
int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
int do_esc_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
int return_code = PX4_OK;
uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
// since we publish multiple at once, make sure the output driver subscribes before we publish
@@ -154,112 +156,3 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
return return_code;
}
static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
{
int return_code = PX4_OK;
hrt_abstime timeout_start = 0;
uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
const battery_status_s &battery = batt_sub.get();
batt_sub.update();
bool batt_connected = battery.connected;
int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
return_code = PX4_ERROR;
goto Out;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
return_code = PX4_ERROR;
goto Out;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
return_code = PX4_ERROR;
goto Out;
}
calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
while (true) {
// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
// sit high.
static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
static constexpr hrt_abstime pwm_high_timeout{3_s};
hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
if (!batt_connected) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
return_code = PX4_ERROR;
goto Out;
}
// PWM was high long enough
break;
}
if (!batt_connected) {
if (batt_sub.update()) {
if (battery.connected) {
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
px4_usleep(50000);
}
Out:
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
if (return_code == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
}
int do_esc_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
int32_t ctrl_alloc = 0;
if (p_ctrl_alloc != PARAM_INVALID) {
param_get(p_ctrl_alloc, &ctrl_alloc);
}
if (ctrl_alloc == 1) {
return do_esc_calibration_ctrl_alloc(mavlink_log_pub);
} else {
return do_esc_calibration_ioctl(mavlink_log_pub);
}
}
src/modules/simulation/pwm_out_sim/PWMSim.cpp
-126
View File
@@ -42,7 +42,6 @@
#include <px4_platform_common/sem.hpp>
PWMSim::PWMSim(bool hil_mode_enabled) :
CDev(PWM_OUTPUT0_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{
_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
@@ -51,8 +50,6 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
_mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
_mixing_output.setIgnoreLockdown(hil_mode_enabled);
CDev::init();
}
void
@@ -66,8 +63,6 @@ PWMSim::Run()
return;
}
SmartLock lock_guard(_lock);
_mixing_output.update();
// check for parameter updates
@@ -121,127 +116,6 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
return false;
}
int
PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
SmartLock lock_guard(_lock);
int ret = OK;
switch (cmd) {
case PWM_SERVO_ARM:
break;
case PWM_SERVO_DISARM:
break;
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (i < OutputModuleInterface::MAX_ACTUATORS && false) {
_mixing_output.minValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (i < OutputModuleInterface::MAX_ACTUATORS && false) {
_mixing_output.maxValue(i) = pwm->values[i];
}
}
break;
}
case PWM_SERVO_SET_UPDATE_RATE:
// PWMSim does not limit the update rate
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = 9999;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = 9999;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = 0;
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.failsafeValue(i);
}
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.disarmedValue(i);
}
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.minValue(i);
}
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.maxValue(i);
}
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
break;
}
case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
// no restrictions on output grouping
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
*(uint32_t *)arg = (1 << channel);
break;
}
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS;
break;
default:
ret = -ENOTTY;
break;
}
return ret;
}
int
PWMSim::task_spawn(int argc, char *argv[])
{
src/modules/simulation/pwm_out_sim/PWMSim.hpp
+1 -4
View File
@@ -52,10 +52,9 @@
#define PARAM_PREFIX "HIL_ACT"
#endif
using namespace time_literals;
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
{
public:
PWMSim(bool hil_mode_enabled);
@@ -72,8 +71,6 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
src/modules/vtol_att_control/standard.cpp
+1 -3
View File
@@ -290,11 +290,9 @@ void Standard::update_transition_state()
mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
}
set_all_motor_state(motor_state::ENABLED);
// set idle speed for MC actuators
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
_flag_idle_mc = true;
}
}
src/modules/vtol_att_control/tailsitter.cpp
+1 -1
View File
@@ -256,7 +256,7 @@ void Tailsitter::update_transition_state()
const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
_flag_idle_mc = true;
}
if (tilt > 0.01f) {
src/modules/vtol_att_control/tiltrotor.cpp
+1 -15
View File
@@ -263,13 +263,7 @@ void Tiltrotor::update_mc_state()
// normal operation
_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
_mc_yaw_weight = 1.0f;
// do thrust compensation only for legacy (static) allocation
if (!_param_sys_ctrl_alloc.get()) {
_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
}
}
}
void Tiltrotor::update_fw_state()
@@ -309,7 +303,6 @@ void Tiltrotor::update_transition_state()
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
// for the first part of the transition all rotors are enabled
set_all_motor_state(motor_state::ENABLED);
// tilt rotors forward up to certain angle
if (_tilt_control <= _param_vt_tilt_trans.get()) {
@@ -357,12 +350,6 @@ void Tiltrotor::update_transition_state()
_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 / _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);
// add minimum throttle for front transition
_thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN);
@@ -378,7 +365,7 @@ void Tiltrotor::update_transition_state()
// set idle speed for rotary wing mode
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
_flag_idle_mc = true;
}
// tilt rotors back once motors are idle
@@ -407,7 +394,6 @@ void Tiltrotor::update_transition_state()
// while we quickly rotate back the motors keep throttle at idle
// turn on all MC motors
set_all_motor_state(motor_state::ENABLED);
_mc_throttle_weight = 0.0f;
_mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f;
src/modules/vtol_att_control/vtol_att_control_main.h
-1
View File
@@ -50,7 +50,6 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
src/modules/vtol_att_control/vtol_att_control_params.c
-46
View File
@@ -39,18 +39,6 @@
* @author Sander Smeets <sander@droneslab.com>
*/
/**
* Idle speed of VTOL when in multicopter mode
*
* @unit us
* @min 900
* @max 2000
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
*
@@ -263,28 +251,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
*/
PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
/**
* The channel number of motors that must be turned off in fixed wing mode.
*
* @min 0
* @max 12345678
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
/**
* The channel number of motors which provide lift during hover.
*
* @min 0
* @max 12345678
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MOT_ID, 0);
/**
* Differential thrust in forwards flight.
*
@@ -336,18 +302,6 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.f);
*/
PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f);
/**
* Enable the usage of AUX outputs for hover motors.
*
* Set this parameter to true if the vehicle's hover motors are connected to the FMU (AUX) port.
* Not required for boards that only have a FMU, and no IO.
* Only applies for standard VTOL and tiltrotor.
*
* @boolean
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0);
/**
* Minimum pitch angle during hover.
*
src/modules/vtol_att_control/vtol_type.cpp
+2 -251
View File
@@ -75,72 +75,14 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_airspeed_validated = _attc->get_airspeed();
_tecs_status = _attc->get_tecs_status();
_land_detected = _attc->get_land_detected();
for (auto &pwm_max : _max_mc_pwm_values.values) {
pwm_max = PWM_DEFAULT_MAX;
}
for (auto &pwm_disarmed : _disarmed_pwm_values.values) {
pwm_disarmed = PWM_MOTOR_OFF;
}
}
bool VtolType::init()
{
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);
if (fd < 0) {
PX4_ERR("can't open %s", dev);
return false;
}
int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);
_current_max_pwm_values = _max_mc_pwm_values;
if (ret != PX4_OK) {
PX4_ERR("failed getting max values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting min values");
px4_close(fd);
return false;
}
ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);
if (ret != PX4_OK) {
PX4_ERR("failed getting disarmed values");
px4_close(fd);
return false;
}
px4_close(fd);
_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
for (int i = 0; i < 8; i++) {
if (_alternate_motor_channel_bitmap & (1 << i)) {
_main_motor_channel_bitmap &= ~(1 << i);
}
}
}
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol);
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol);
return true;
}
void VtolType::parameters_update()
@@ -156,13 +98,11 @@ void VtolType::parameters_update()
void VtolType::update_mc_state()
{
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
_flag_idle_mc = true;
}
resetAccelToPitchPitchIntegrator();
VtolType::set_all_motor_state(motor_state::ENABLED);
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
@@ -185,14 +125,12 @@ void VtolType::update_mc_state()
void VtolType::update_fw_state()
{
if (_flag_idle_mc) {
_flag_idle_mc = !set_idle_fw();
_flag_idle_mc = false;
}
resetAccelToPitchPitchIntegrator();
_last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
VtolType::set_alternate_motor_state(motor_state::DISABLED);
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_mc_roll_weight = 0.0f;
@@ -354,193 +292,6 @@ void VtolType::check_quadchute_condition()
}
}
bool VtolType::set_idle_mc()
{
if (_param_sys_ctrl_alloc.get()) {
return true;
}
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(_param_vt_mot_id.get()))) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = _min_mc_pwm_values.values[i];
}
pwm_values.channel_count++;
}
return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM);
}
bool VtolType::set_idle_fw()
{
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(_param_vt_mot_id.get()))) {
pwm_values.values[i] = PWM_DEFAULT_MIN;
} else {
pwm_values.values[i] = _min_mc_pwm_values.values[i];
}
pwm_values.channel_count++;
}
return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM);
}
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
{
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
return false;
}
int ret;
if (type == pwm_limit_type::TYPE_MINIMUM) {
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
} else {
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
}
px4_close(fd);
if (ret != OK) {
PX4_DEBUG("failed setting max values");
return false;
}
return true;
}
void VtolType::set_all_motor_state(const motor_state target_state, const int value)
{
if (_param_sys_ctrl_alloc.get()) {
return;
}
set_main_motor_state(target_state, value);
set_alternate_motor_state(target_state, value);
}
void VtolType::set_main_motor_state(const motor_state target_state, const int value)
{
if (_param_sys_ctrl_alloc.get()) {
return;
}
if (_main_motor_state != target_state || target_state == motor_state::VALUE) {
if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) {
_main_motor_state = target_state;
}
}
}
void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
{
if (_param_sys_ctrl_alloc.get()) {
return;
}
if (_alternate_motor_state != target_state || target_state == motor_state::VALUE) {
if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) {
_alternate_motor_state = target_state;
}
}
}
bool VtolType::set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value)
{
switch (target_state) {
case motor_state::ENABLED:
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i];
}
}
break;
case motor_state::DISABLED:
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = _disarmed_pwm_values.values[i];
}
}
break;
case motor_state::IDLE:
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get();
}
}
break;
case motor_state::VALUE:
for (int i = 0; i < num_outputs_max; i++) {
if (is_channel_set(i, channel_bitmap)) {
_current_max_pwm_values.values[i] = value;
}
}
break;
}
_current_max_pwm_values.channel_count = num_outputs_max;
return apply_pwm_limits(_current_max_pwm_values, pwm_limit_type::TYPE_MAXIMUM);
}
int VtolType::generate_bitmap_from_channel_numbers(const int channels)
{
int channel_bitmap = 0;
int channel_numbers = channels;
int tmp;
for (int i = 0; i < num_outputs_max; ++i) {
tmp = channel_numbers % 10;
if (tmp == 0) {
break;
}
channel_bitmap |= 1 << (tmp - 1);
channel_numbers = channel_numbers / 10;
}
return channel_bitmap;
}
bool VtolType::is_channel_set(const int channel, const int bitmap)
{
return bitmap & (1 << channel);
}
float VtolType::pusher_assist()
{
// Altitude above ground is distance sensor altitude if available, otherwise local z-position
src/modules/vtol_att_control/vtol_type.h
+1 -83
View File
@@ -44,7 +44,6 @@
#define VTOL_TYPE_H
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mathlib/mathlib.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <px4_platform_common/module_params.h>
@@ -78,17 +77,6 @@ enum VtolForwardActuationMode {
ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND
};
// these are states that can be applied to a selection of multirotor motors.
// e.g. if we need to shut off some motors after transitioning to fixed wing mode
// we can individually disable them while others might still need to be enabled to produce thrust.
// we can select the target motors via VT_FW_MOT_OFFID
enum class motor_state {
ENABLED = 0, // motor max pwm will be set to the standard max pwm value
DISABLED, // motor max pwm will be set to a value that shuts the motor off
IDLE, // motor max pwm will be set to VT_IDLE_PWM_MC
VALUE // motor max pwm will be set to a specific value provided, see set_motor_state()
};
/**
* @brief Used to specify if min or max pwm values should be altered
*/
@@ -99,7 +87,7 @@ enum class pwm_limit_type {
class VtolAttitudeControl;
class VtolType: public ModuleParams
class VtolType : public ModuleParams
{
public:
@@ -233,9 +221,6 @@ protected:
bool _tecs_running = false;
hrt_abstime _tecs_running_ts = 0;
motor_state _main_motor_state = motor_state::DISABLED;
motor_state _alternate_motor_state = motor_state::DISABLED;
hrt_abstime _last_loop_ts = 0;
float _transition_dt = 0;
@@ -243,29 +228,6 @@ protected:
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.
*
* @return true on success
*/
bool set_idle_mc();
/**
* @brief Sets mc motor minimum pwm to PWM_MIN which ensures that the
* motors stop spinning on zero throttle in fw mode.
*
* @return true on success
*/
bool set_idle_fw();
void set_all_motor_state(motor_state target_state, int value = 0);
void set_main_motor_state(motor_state target_state, int value = 0);
void set_alternate_motor_state(motor_state target_state, int value = 0);
float update_and_get_backtransition_pitch_sp();
SlewRate<float> _spoiler_setpoint_with_slewrate;
@@ -304,57 +266,13 @@ protected:
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_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,
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
)
private:
hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle
/**
* @brief Stores the max pwm values given by the system.
*/
struct pwm_output_values _min_mc_pwm_values {};
struct pwm_output_values _max_mc_pwm_values {};
struct pwm_output_values _disarmed_pwm_values {};
struct pwm_output_values _current_max_pwm_values {};
int32_t _main_motor_channel_bitmap = 0;
int32_t _alternate_motor_channel_bitmap = 0;
/**
* @brief Adjust minimum/maximum pwm values for the output channels.
*
* @param pwm_output_values Struct containing the limit values for each channel
* @param[in] type Specifies if min or max limits are adjusted.
*
* @return True on success.
*/
bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type);
/**
* @brief Determines if channel is set in a bitmap.
*
* @param[in] channel The channel
* @param[in] bitmap The bitmap to check on.
*
* @return True if set, false otherwise.
*/
bool is_channel_set(const int channel, const int bitmap);
// generates a bitmap from a number format, e.g. 1235 -> first, second, third and fifth bits should be set.
int generate_bitmap_from_channel_numbers(const int channels);
bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value);
void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; }
bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; };