fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL preserve PWM min and max when enabling/disabling actuators

Browse Source
This commit is contained in:
Daniel Agar 2018-03-05 16:02:22 -05:00 committed by Roman Bapst
parent a78ab02144
commit 00f98c64db
8 changed files with 113 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,6 @@
Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc),
_flag_enable_mc_motors(true),
_pusher_throttle(0.0f),
_reverse_output(0.0f),
_airspeed_trans_blend_margin(0.0f)
@ -300,11 +299,9 @@ void Standard::update_transition_state()
}
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// in back transition we need to start the MC motors again
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
_flag_enable_mc_motors = !enable_mc_motors();
}
}
@ -322,9 +319,7 @@ void Standard::update_mc_state()
// enable MC motors here in case we transitioned directly to MC mode
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
_flag_enable_mc_motors = !enable_mc_motors();
}
// if the thrust scale param is zero or the drone is on manual mode,
@ -403,11 +398,9 @@ void Standard::update_fw_state()
{
VtolType::update_fw_state();
// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
// stop MC motors in FW mode
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw(); // force them to stop, not just idle
_flag_enable_mc_motors = true;
_flag_enable_mc_motors = disable_mc_motors();
}
}
@ -495,36 +488,3 @@ Standard::waiting_on_tecs()
// keep thrust from transition
_v_att_sp->thrust = _pusher_throttle;
};
/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(unsigned pwm_value)
{
int ret;
unsigned servo_count;
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
}
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
PX4_WARN("failed setting max values");
}
px4_close(fd);
}

6
src/modules/vtol_att_control/standard.h
View File

@ -98,13 +98,13 @@ private:
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
} _vtol_schedule;
bool _flag_enable_mc_motors;
// TODO: replace with vtol_type's flag_idle_mc
bool _flag_enable_mc_motors{true};
float _pusher_throttle;
float _reverse_output;
float _airspeed_trans_blend_margin;
void set_max_mc(unsigned pwm_value);
virtual void parameters_update();
};
#endif

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

@ -214,8 +214,7 @@ void Tailsitter::update_transition_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
flag_idle_mc = enable_mc_motors();
}
// create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
@ -261,8 +260,7 @@ void Tailsitter::update_mc_state()
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
flag_idle_mc = enable_mc_motors();
}
}
@ -271,8 +269,7 @@ void Tailsitter::update_fw_state()
VtolType::update_fw_state();
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
flag_idle_mc = !disable_mc_motors();
}
}

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

@ -104,7 +104,7 @@ Tiltrotor::parameters_update()
_params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
}
int Tiltrotor::get_motor_off_channels(int channels)
uint32_t Tiltrotor::get_motor_off_channels(int channels)
{
int channel_bitmap = 0;
@ -251,8 +251,7 @@ void Tiltrotor::update_mc_state()
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
flag_idle_mc = enable_mc_motors();
}
}
@ -270,8 +269,7 @@ void Tiltrotor::update_fw_state()
// adjust idle for fixed wing flight
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
flag_idle_mc = !disable_mc_motors();
}
}
@ -347,8 +345,7 @@ void Tiltrotor::update_transition_state()
}
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
flag_idle_mc = enable_mc_motors();
}
// tilt rotors back
@ -447,8 +444,6 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
break;
}
int ret;
unsigned servo_count;
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
@ -456,9 +451,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
PX4_WARN("can't open %s", dev);
}
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_max_values;
memset(&pwm_max_values, 0, sizeof(pwm_max_values));
struct pwm_output_values pwm_max_values = {};
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (is_motor_off_channel(i)) {
@ -471,10 +464,10 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
pwm_max_values.channel_count = _params->vtol_motor_count;
}
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
if (ret != OK) {
PX4_WARN("failed setting max values");
PX4_ERR("failed setting max values");
}
px4_close(fd);

2
src/modules/vtol_att_control/tiltrotor.h
View File

@ -111,7 +111,7 @@ private:
/**
* Return a bitmap of channels that should be turned off in fixed wing mode.
*/
int get_motor_off_channels(const int channels);
uint32_t get_motor_off_channels(const int channels);
/**
* Return true if the motor channel is off in fixed wing mode.

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

@ -595,9 +595,6 @@ void VtolAttitudeControl::task_main()
parameters_update(); // initialize parameter cache
// make sure we start with idle in mc mode
_vtol_type->set_idle_mc();
/* wakeup source*/
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _actuator_inputs_mc;

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

@ -67,7 +67,9 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_land_detected = _attc->get_land_detected();
_params = _attc->get_params();
flag_idle_mc = true;
for (auto &pwm_max : _max_mc_pwm_values.values) {
pwm_max = PWM_DEFAULT_MAX;
}
}
VtolType::~VtolType()
@ -75,71 +77,6 @@ VtolType::~VtolType()
}
/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
}
unsigned servo_count;
int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
PX4_WARN("failed setting min values");
}
px4_close(fd);
flag_idle_mc = true;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_WARN("can't open %s", dev);
}
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = PWM_MOTOR_OFF;
pwm_values.channel_count++;
}
int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
PX4_WARN("failed setting min values");
}
px4_close(fd);
}
void VtolType::update_mc_state()
{
// copy virtual attitude setpoint to real attitude setpoint
@ -263,5 +200,87 @@ void VtolType::check_quadchute_condition()
}
}
}
}
bool
VtolType::disable_mc_motors()
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_ERR("can't open %s", dev);
return false;
}
// first save the current max values
struct pwm_output_values max_pwm_values = {};
int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&max_pwm_values);
if (ret == OK) {
_max_mc_pwm_values = max_pwm_values;
} else {
PX4_ERR("failed getting max values");
px4_close(fd);
return false;
}
// now get the disarmed PWM values
pwm_output_values disarmed_pwm_values = {};
ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&disarmed_pwm_values);
if (ret == OK) {
// finally disable by setting the MC motors max to the disarmed value
max_pwm_values.channel_count = _params->vtol_motor_count;
for (int i = 0; i < _params->vtol_motor_count; i++) {
max_pwm_values.values[i] = disarmed_pwm_values.values[i];
}
ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&max_pwm_values);
if (ret != OK) {
PX4_ERR("failed setting max values");
}
} else {
PX4_ERR("failed getting max values");
}
px4_close(fd);
return (ret == PX4_OK);
}
bool
VtolType::enable_mc_motors()
{
const char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0);
if (fd < 0) {
PX4_ERR("can't open %s", dev);
return false;
}
struct pwm_output_values pwm_values = {};
pwm_values.channel_count = _params->vtol_motor_count;
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = _max_mc_pwm_values.values[i];
}
int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
PX4_ERR("failed setting max values");
}
px4_close(fd);
return (ret == PX4_OK);
}

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

@ -45,6 +45,7 @@
#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
struct Params {
int32_t idle_pwm_mc; // pwm value for idle in mc mode
@ -182,10 +183,17 @@ protected:
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;
bool _tecs_running = false;
hrt_abstime _tecs_running_ts = 0;
struct pwm_output_values _max_mc_pwm_values {};
bool enable_mc_motors();
bool disable_mc_motors();
};
#endif