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

vtol_att avoid unnecessary work and delete unused

This commit is contained in:
Daniel Agar 2017-12-31 20:23:51 -05:00 committed by Lorenz Meier
parent d3a220f807
commit 66f614435f
12 changed files with 178 additions and 565 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vtol_vehicle_status.msg
View File

@ -4,11 +4,9 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1
uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
uint8 VEHICLE_VTOL_STATE_MC = 3
uint8 VEHICLE_VTOL_STATE_FW = 4
uint8 VEHICLE_VTOL_STATE_EXTERNAL = 5
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
float32 airspeed_tot # Estimated airspeed over control surfaces

1
src/modules/sdlog2/sdlog2.c
View File

@ -1567,7 +1567,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VTOL VEHICLE STATUS --- */
if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) {
log_msg.msg_type = LOG_VTOL_MSG;
log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
log_msg.body.log_VTOL.rw_mode = buf.vtol_status.vtol_in_rw_mode;
log_msg.body.log_VTOL.trans_mode = buf.vtol_status.vtol_in_trans_mode;
log_msg.body.log_VTOL.failsafe_mode = buf.vtol_status.vtol_transition_failsafe;

1
src/modules/sdlog2/sdlog2_messages.h
View File

@ -466,7 +466,6 @@ struct log_ENCD_s {
/* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 43
struct log_VTOL_s {
float airspeed_tot;
uint8_t rw_mode;
uint8_t trans_mode;
uint8_t failsafe_mode;

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

@ -50,13 +50,10 @@
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc),
_airspeed_tot(0.0f),
_min_front_trans_dur(0.5f),
_thrust_transition_start(0.0f),
_yaw_transition(0.0f),
_pitch_transition_start(0.0f),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
_pitch_transition_start(0.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@ -133,7 +130,6 @@ void Tailsitter::update_vtol_state()
if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
@ -334,9 +330,6 @@ void Tailsitter::update_transition_state()
}
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
@ -358,58 +351,6 @@ void Tailsitter::waiting_on_tecs()
_v_att_sp->thrust = _thrust_transition;
}
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->indicated_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P, 1.0f, _params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed / eta - airspeed) * 2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !PX4_ISFINITE(_airspeed->indicated_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
-1.0f, 1.0f);
}
void Tailsitter::update_mc_state()
{
VtolType::update_mc_state();
@ -504,9 +445,5 @@ void Tailsitter::fill_actuator_outputs()
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
break;
case EXTERNAL:
// not yet implemented, we are switching brute force at the moment
break;
}
}

16
src/modules/vtol_att_control/tailsitter.h
View File

@ -96,8 +96,6 @@ private:
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
float _airspeed_tot; /** speed estimation for propwash controlled surfaces */
/** not sure about it yet ?! **/
float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */
@ -105,20 +103,6 @@ private:
float _yaw_transition; // yaw angle in which transition will take place
float _pitch_transition_start; // pitch angle at the start of transition (tailsitter)
/** should this anouncement stay? **/
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/**
* Speed estimation for propwash controlled surfaces.
*/
void calc_tot_airspeed();
/** is this one still needed? */
void scale_mc_output();
/**
* Update parameters.
*/

2
src/modules/vtol_att_control/tailsitter_params.c
View File

@ -39,8 +39,6 @@
* @author David Vorsin <davidvorsin@gmail.com>
*/
#include <systemlib/param/param.h>
/**
* Duration of front transition phase 2
*

2
src/modules/vtol_att_control/tiltrotor_params.c
View File

@ -38,8 +38,6 @@
* @author Roman Bapst <roman@px4.io>
*/
#include <systemlib/param/param.h>
/**
* Position of tilt servo in mc mode
*

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

@ -57,64 +57,9 @@ VtolAttitudeControl *g_control;
/**
* Constructor
*/
VtolAttitudeControl::VtolAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
// mavlink log
_mavlink_log_pub(nullptr),
//init subscription handlers
_v_att_sub(-1),
_v_att_sp_sub(-1),
_mc_virtual_att_sp_sub(-1),
_fw_virtual_att_sp_sub(-1),
_mc_virtual_v_rates_sp_sub(-1),
_fw_virtual_v_rates_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_airspeed_sub(-1),
_battery_status_sub(-1),
_vehicle_cmd_sub(-1),
_tecs_status_sub(-1),
_land_detected_sub(-1),
//init publication handlers
_actuators_0_pub(nullptr),
_actuators_1_pub(nullptr),
_vtol_vehicle_status_pub(nullptr),
_v_rates_sp_pub(nullptr),
_v_att_sp_pub(nullptr),
_v_cmd_ack_pub(nullptr),
_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
_abort_front_transition(false)
VtolAttitudeControl::VtolAttitudeControl()
{
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
memset(&_v_att, 0, sizeof(_v_att));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp));
memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
memset(&_airspeed, 0, sizeof(_airspeed));
memset(&_batt_status, 0, sizeof(_batt_status));
memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
memset(&_tecs_status, 0, sizeof(_tecs_status));
memset(&_land_detected, 0, sizeof(_land_detected));
_params.idle_pwm_mc = PWM_DEFAULT_MIN;
_params.vtol_motor_count = 0;
@ -122,13 +67,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
_params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM");
_params_handles.power_max = param_find("VT_POWER_MAX");
_params_handles.prop_eff = param_find("VT_PROP_EFF");
_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
_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");
@ -247,32 +186,6 @@ void VtolAttitudeControl::actuator_controls_fw_poll()
}
}
/**
* Check for attitude rates setpoint from mc attitude controller
*/
void VtolAttitudeControl::vehicle_rates_sp_mc_poll()
{
bool updated;
orb_check(_mc_virtual_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &_mc_virtual_v_rates_sp);
}
}
/**
* Check for attitude rates setpoint from fw attitude controller
*/
void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
{
bool updated;
orb_check(_fw_virtual_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &_fw_virtual_v_rates_sp);
}
}
/**
* Check for airspeed updates.
*/
@ -287,21 +200,6 @@ VtolAttitudeControl::vehicle_airspeed_poll()
}
}
/**
* Check for attitude set points update.
*/
void
VtolAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
/**
* Check for attitude update.
*/
@ -317,38 +215,6 @@ VtolAttitudeControl::vehicle_attitude_poll()
}
}
/**
* Check for battery updates.
*/
void
VtolAttitudeControl::vehicle_battery_poll()
{
bool updated;
orb_check(_battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), _battery_status_sub, &_batt_status);
}
}
/**
* Check for parameter updates.
*/
void
VtolAttitudeControl::parameters_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
/**
* Check for sensor updates.
*/
@ -410,7 +276,6 @@ VtolAttitudeControl::mc_virtual_att_sp_poll()
if (updated) {
orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub, &_mc_virtual_att_sp);
}
}
/**
@ -426,7 +291,6 @@ VtolAttitudeControl::fw_virtual_att_sp_poll()
if (updated) {
orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub, &_fw_virtual_att_sp);
}
}
/**
@ -575,34 +439,10 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.vtol_fw_permanent_stab, &l);
_vtol_vehicle_status.fw_permanent_stab = (l == 1);
/* vtol mc mode min airspeed */
param_get(_params_handles.mc_airspeed_min, &v);
_params.mc_airspeed_min = v;
/* vtol mc mode max airspeed */
param_get(_params_handles.mc_airspeed_max, &v);
_params.mc_airspeed_max = v;
/* vtol mc mode trim airspeed */
param_get(_params_handles.mc_airspeed_trim, &v);
_params.mc_airspeed_trim = v;
/* vtol pitch trim for fw mode */
param_get(_params_handles.fw_pitch_trim, &v);
_params.fw_pitch_trim = v;
/* vtol maximum power engine can produce */
param_get(_params_handles.power_max, &v);
_params.power_max = v;
/* vtol propeller efficiency factor */
param_get(_params_handles.prop_eff, &v);
_params.prop_eff = v;
/* vtol total airspeed estimate low pass gain */
param_get(_params_handles.arsp_lp_gain, &v);
_params.arsp_lp_gain = v;
param_get(_params_handles.vtol_type, &l);
_params.vtol_type = l;
@ -660,11 +500,22 @@ VtolAttitudeControl::parameters_update()
*/
void VtolAttitudeControl::fill_mc_att_rates_sp()
{
_v_rates_sp.timestamp = _mc_virtual_v_rates_sp.timestamp;
_v_rates_sp.roll = _mc_virtual_v_rates_sp.roll;
_v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch;
_v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw;
_v_rates_sp.thrust = _mc_virtual_v_rates_sp.thrust;
bool updated;
orb_check(_mc_virtual_v_rates_sp_sub, &updated);
if (updated) {
vehicle_rates_setpoint_s v_rates_sp;
if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
// publish the attitude rates setpoint
if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
}
}
}
}
/**
@ -672,22 +523,21 @@ void VtolAttitudeControl::fill_mc_att_rates_sp()
*/
void VtolAttitudeControl::fill_fw_att_rates_sp()
{
_v_rates_sp.timestamp = _fw_virtual_v_rates_sp.timestamp;
_v_rates_sp.roll = _fw_virtual_v_rates_sp.roll;
_v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch;
_v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw;
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
}
bool updated;
orb_check(_fw_virtual_v_rates_sp_sub, &updated);
void VtolAttitudeControl::publish_att_sp()
{
if (_v_att_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
if (updated) {
vehicle_rates_setpoint_s v_rates_sp;
if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
// publish the attitude rates setpoint
if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
} else {
/* advertise and publish */
_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
}
}
}
}
@ -708,7 +558,6 @@ void VtolAttitudeControl::task_main()
_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -716,7 +565,6 @@ void VtolAttitudeControl::task_main()
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
@ -730,48 +578,11 @@ void VtolAttitudeControl::task_main()
_vtol_type->set_idle_mc();
/* wakeup source*/
px4_pollfd_struct_t fds[2] = {}; /*input_mc, input_fw, parameters*/
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _actuator_inputs_mc;
fds[0].events = POLLIN;
fds[1].fd = _actuator_inputs_fw;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/*Advertise/Publish vtol vehicle status*/
_vtol_vehicle_status.timestamp = hrt_absolute_time();
if (_vtol_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
}
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
}
/* only update parameters if they changed */
bool params_updated = false;
orb_check(_params_sub, &params_updated);
@ -785,25 +596,47 @@ void VtolAttitudeControl::task_main()
parameters_update();
}
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
vehicle_manual_poll(); //Check for changes in manual inputs.
vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
vehicle_attitude_poll(); //Check for changes in attitude
actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
vehicle_rates_sp_mc_poll();
vehicle_rates_sp_fw_poll();
parameters_update_poll();
vehicle_local_pos_poll(); // Check for new sensor values
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_ERR("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
// run vtol_att on MC actuator publications, unless in full FW mode
switch (_vtol_type->get_mode()) {
case TRANSITION_TO_FW:
case TRANSITION_TO_MC:
case ROTARY_WING:
fds[0].fd = _actuator_inputs_mc;
break;
case FIXED_WING:
fds[0].fd = _actuator_inputs_fw;
break;
}
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_attitude_poll();
vehicle_local_pos_poll();
vehicle_local_pos_sp_poll();
pos_sp_triplet_poll();
vehicle_airspeed_poll();
vehicle_battery_poll();
vehicle_cmd_poll();
tecs_status_poll();
land_detected_poll();
actuator_controls_fw_poll();
actuator_controls_mc_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
@ -826,67 +659,58 @@ void VtolAttitudeControl::task_main()
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == ROTARY_WING) {
mc_virtual_att_sp_poll();
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
_vtol_vehicle_status.vtol_in_trans_mode = false;
// got data from mc attitude controller
if (fds[0].revents & POLLIN) {
_vtol_type->update_mc_state();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == FIXED_WING) {
fw_virtual_att_sp_poll();
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
_vtol_vehicle_status.vtol_in_trans_mode = false;
// got data from fw attitude controller
if (fds[1].revents & POLLIN) {
vehicle_manual_poll();
_vtol_type->update_fw_state();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) {
mc_virtual_att_sp_poll();
fw_virtual_att_sp_poll();
// 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() == TRANSITION_TO_FW);
bool got_new_data = false;
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
got_new_data = true;
}
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
got_new_data = true;
}
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
fill_mc_att_rates_sp();
}
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
publish_att_sp();
_vtol_type->fill_actuator_outputs();
/* Only publish if the proper mode(s) are enabled */
if (_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled) {
if (_v_att_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
} else {
/* advertise and publish */
_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
}
if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
@ -902,12 +726,14 @@ void VtolAttitudeControl::task_main()
}
}
// publish the attitude rates setpoint
if (_v_rates_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
/*Advertise/Publish vtol vehicle status*/
_vtol_vehicle_status.timestamp = hrt_absolute_time();
if (_vtol_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
}
}
@ -923,7 +749,7 @@ VtolAttitudeControl::start()
/* start the task */
_control_task = px4_task_spawn_cmd("vtol_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
SCHED_PRIORITY_ATTITUDE_CONTROL + 1,
1230,
(px4_main_t)&VtolAttitudeControl::task_main_trampoline,
nullptr);

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

@ -59,13 +59,10 @@
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/tecs_status.h>
@ -80,7 +77,6 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/uORB.h>
#include "tiltrotor.h"
#include "tailsitter.h"
@ -101,104 +97,90 @@ public:
bool is_fixed_wing_requested();
void abort_front_transition(const char *reason);
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;}
struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;}
struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;}
struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;}
struct tecs_status_s *get_tecs_status() {return &_tecs_status;}
struct vehicle_attitude_s *get_att() {return &_v_att;}
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;}
struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;}
struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;}
struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;}
struct vehicle_local_position_s *get_local_pos() {return &_local_pos;}
struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;}
struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;}
struct airspeed_s *get_airspeed() {return &_airspeed;}
struct battery_status_s *get_batt_status() {return &_batt_status;}
struct tecs_status_s *get_tecs_status() {return &_tecs_status;}
struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
struct Params *get_params() {return &_params;}
private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
orb_advert_t _mavlink_log_pub; // mavlink log uORB handle
bool _task_should_exit{false};
int _control_task{-1}; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_att_sp_sub;
int _fw_virtual_att_sp_sub;
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _v_control_mode_sub; //vehicle control mode subscription
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _local_pos_sub; // sensor subscription
int _local_pos_sp_sub; // setpoint subscription
int _pos_sp_triplet_sub; // local position setpoint subscription
int _airspeed_sub; // airspeed subscription
int _battery_status_sub; // battery status subscription
int _vehicle_cmd_sub;
int _tecs_status_sub;
int _land_detected_sub;
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs
int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs
int _airspeed_sub{-1}; // airspeed subscription
int _fw_virtual_att_sp_sub{-1};
int _fw_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription
int _land_detected_sub{-1};
int _local_pos_sp_sub{-1}; // setpoint subscription
int _local_pos_sub{-1}; // sensor subscription
int _manual_control_sp_sub{-1}; //manual control setpoint subscription
int _mc_virtual_att_sp_sub{-1};
int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription
int _params_sub{-1}; //parameter updates subscription
int _pos_sp_triplet_sub{-1}; // local position setpoint subscription
int _tecs_status_sub{-1};
int _v_att_sp_sub{-1}; //vehicle attitude setpoint subscription
int _v_att_sub{-1}; //vehicle attitude subscription
int _v_control_mode_sub{-1}; //vehicle control mode subscription
int _vehicle_cmd_sub{-1};
//handlers for publishers
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
orb_advert_t _v_att_sp_pub;
orb_advert_t _v_cmd_ack_pub;
orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
orb_advert_t _v_att_sp_pub{nullptr};
orb_advert_t _v_cmd_ack_pub{nullptr};
orb_advert_t _v_rates_sp_pub{nullptr};
orb_advert_t _vtol_vehicle_status_pub{nullptr};
orb_advert_t _actuators_1_pub{nullptr};
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct vehicle_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint
struct vehicle_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
struct vehicle_local_position_s _local_pos;
struct vehicle_local_position_setpoint_s _local_pos_sp;
struct position_setpoint_triplet_s _pos_sp_triplet;
struct airspeed_s _airspeed; // airspeed
struct battery_status_s _batt_status; // battery status
struct vehicle_command_s _vehicle_cmd;
struct tecs_status_s _tecs_status;
struct vehicle_land_detected_s _land_detected;
Params _params; // struct holding the parameters
vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint
vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint
vehicle_attitude_setpoint_s _mc_virtual_att_sp{}; // virtual mc attitude setpoint
actuator_controls_s _actuators_fw_in{}; //actuator controls from fw_att_control
actuator_controls_s _actuators_mc_in{}; //actuator controls from mc_att_control
actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
airspeed_s _airspeed{}; // airspeed
manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude
vehicle_command_s _vehicle_cmd{};
vehicle_control_mode_s _v_control_mode{}; //vehicle control mode
vehicle_land_detected_s _land_detected{};
vehicle_local_position_s _local_pos{};
vehicle_local_position_setpoint_s _local_pos_sp{};
vtol_vehicle_status_s _vtol_vehicle_status{};
Params _params{}; // struct holding the parameters
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
param_t fw_pitch_trim;
param_t power_max;
param_t prop_eff;
param_t arsp_lp_gain;
param_t vtol_type;
param_t elevons_mc_lock;
param_t fw_min_alt;
@ -210,45 +192,42 @@ private:
param_t wv_takeoff;
param_t wv_loiter;
param_t wv_land;
} _params_handles;
} _params_handles{};
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
int _transition_command;
bool _abort_front_transition;
int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC};
bool _abort_front_transition{false};
VtolType *_vtol_type = nullptr; // base class for different vtol types
VtolType *_vtol_type{nullptr}; // base class for different vtol types
//*****************Member functions***********************************************************************
void task_main(); //main task
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void land_detected_poll();
void tecs_status_poll();
void vehicle_attitude_poll(); //Check for attitude updates.
void vehicle_cmd_poll();
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void mc_virtual_att_sp_poll();
void fw_virtual_att_sp_poll();
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void vehicle_rates_sp_mc_poll();
void vehicle_rates_sp_fw_poll();
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void fw_virtual_att_sp_poll();
void mc_virtual_att_sp_poll();
void pos_sp_triplet_poll(); // Check for changes in position setpoint values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates.
void vehicle_attitude_poll(); //Check for attitude updates.
void vehicle_battery_poll(); // Check for battery updates
void vehicle_cmd_poll();
void tecs_status_poll();
void land_detected_poll();
void parameters_update_poll(); //Check if parameters have changed
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values
int parameters_update(); //Update local paraemter cache
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void handle_command();
void publish_att_sp();
};
#endif

83
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -62,48 +62,6 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT, 0);
*/
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
/**
* Minimum airspeed in multicopter mode
*
* This is the minimum speed of the air flowing over the control surfaces.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @increment 0.5
* @decimal 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f);
/**
* Maximum airspeed in multicopter mode
*
* This is the maximum speed of the air flowing over the control surfaces.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @increment 0.5
* @decimal 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f);
/**
* Trim airspeed when in multicopter mode
*
* This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @increment 0.5
* @decimal 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f);
/**
* Permanent stabilization in fw mode
*
@ -128,47 +86,6 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
*/
PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f);
/**
* Motor max power
*
* Indicates the maximum power the motor is able to produce. Used to calculate
* propeller efficiency map.
*
* @unit W
* @min 1
* @max 10000
* @increment 1
* @decimal 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f);
/**
* Propeller efficiency parameter
*
* Influences propeller efficiency at different power settings. Should be tuned beforehand.
*
* @min 0.0
* @max 1.0
* @increment 0.01
* @decimal 3
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
/**
* Total airspeed estimate low-pass filter gain
*
* Gain for tuning the low-pass filter for the total airspeed estimate
*
* @min 0.0
* @max 1.0
* @increment 0.01
* @decimal 3
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
*

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

@ -54,10 +54,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_v_att_sp = _attc->get_att_sp();
_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
_v_rates_sp = _attc->get_rates_sp();
_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
_manual_control_sp = _attc->get_manual_control_sp();
_v_control_mode = _attc->get_control_mode();
_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
_actuators_out_0 = _attc->get_actuators_out0();
@ -67,7 +63,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_local_pos = _attc->get_local_pos();
_local_pos_sp = _attc->get_local_pos_sp();
_airspeed = _attc->get_airspeed();
_batt_status = _attc->get_batt_status();
_tecs_status = _attc->get_tecs_status();
_land_detected = _attc->get_land_detected();
_params = _attc->get_params();

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

@ -49,13 +49,7 @@
struct Params {
int32_t idle_pwm_mc; // pwm value for idle in mc mode
int32_t vtol_motor_count; // number of motors
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
int32_t vtol_type;
int32_t elevons_mc_lock; // lock elevons in multicopter mode
float fw_min_alt; // minimum relative altitude for FW mode (QuadChute)
@ -74,8 +68,7 @@ enum mode {
TRANSITION_TO_FW = 1,
TRANSITION_TO_MC = 2,
ROTARY_WING = 3,
FIXED_WING = 4,
EXTERNAL = 5
FIXED_WING = 4
};
enum vtol_type {
@ -116,11 +109,6 @@ public:
*/
virtual void update_fw_state();
/**
* Update external state.
*/
virtual void update_external_state() {}
/**
* Write control values to actuator output topics.
*/
@ -157,10 +145,6 @@ protected:
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint
struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
@ -170,7 +154,6 @@ protected:
struct vehicle_local_position_s *_local_pos;
struct vehicle_local_position_setpoint_s *_local_pos_sp;
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct tecs_status_s *_tecs_status;
struct vehicle_land_detected_s *_land_detected;