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

merged with caipi_airspeed_scale

This commit is contained in:
tumbili 2014-12-10 21:20:53 +01:00
parent d0fb862a01 945c8df18b
commit 9bf19c8d1e
2 changed files with 85 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,6 +63,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -107,6 +108,7 @@ private:
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _armed_sub; //arming status subscription
int _airspeed_sub; // airspeed subscription
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
@ -130,18 +132,26 @@ private:
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 actuator_armed_s _armed; //actuator arming status
struct airspeed_s _airspeed; // airspeed
struct {
param_t idle_pwm_mc; //pwm value for idle in mc mode
param_t vtol_motor_count;
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
} _params;
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
} _params_handles;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/* 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
@ -161,6 +171,7 @@ private:
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_airspeed_poll(); // Check for changes in airspeed
void parameters_update_poll(); //Check if parameters have changed
int parameters_update(); //Update local paraemter cache
void fill_mc_att_control_output(); //write mc_att_control results to actuator message
@ -169,6 +180,7 @@ private:
void fill_fw_att_rates_sp();
void set_idle_fw();
void set_idle_mc();
void scale_mc_output();
};
namespace VTOL_att_control
@ -192,6 +204,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_airspeed_sub(-1),
//init publication handlers
_actuators_0_pub(-1),
@ -199,7 +212,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
_vtol_vehicle_status_pub(-1),
_v_rates_sp_pub(-1),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control"))
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
{
flag_idle_mc = true;
@ -219,12 +233,16 @@ VtolAttitudeControl::VtolAttitudeControl() :
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
memset(&_armed, 0, sizeof(_armed));
memset(&_airspeed,0,sizeof(_airspeed));
_params.idle_pwm_mc = PWM_LOWEST_MIN;
_params.vtol_motor_count = 0;
_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
/* fetch initial parameter values */
parameters_update();
@ -352,6 +370,19 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
}
}
/**
* Check for airspeed updates.
*/
void
VtolAttitudeControl::vehicle_airspeed_poll() {
bool updated;
orb_check(_airspeed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed);
}
}
/**
* Check for parameter updates.
*/
@ -376,12 +407,25 @@ VtolAttitudeControl::parameters_update_poll()
int
VtolAttitudeControl::parameters_update()
{
float v;
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
/* vtol motor count */
param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
/* 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;
return OK;
}
@ -496,6 +540,34 @@ void VtolAttitudeControl::set_idle_mc()
close(fd);
}
void
VtolAttitudeControl::scale_mc_output() {
// scale around tuning airspeed
float airspeed;
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _params.mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
// prevent numerical drama by requiring 0.5 m/s minimal speed
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
* 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
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@ -516,6 +588,7 @@ void VtolAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@ -580,6 +653,7 @@ void VtolAttitudeControl::task_main()
vehicle_rates_sp_mc_poll();
vehicle_rates_sp_fw_poll();
parameters_update_poll();
vehicle_airspeed_poll();
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
_vtol_vehicle_status.vtol_in_rw_mode = true;
@ -593,6 +667,8 @@ void VtolAttitudeControl::task_main()
if (fds[0].revents & POLLIN) {
vehicle_manual_poll(); /* update remote input */
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
// scale pitch control with airspeed
scale_mc_output();
fill_mc_att_control_output();
fill_mc_att_rates_sp();

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

@ -3,4 +3,11 @@
// number of engines
PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
// idle pwm in multicopter mode
PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
// min airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
// max airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
// trim airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);