diff options
-rw-r--r-- | src/modules/vtol_att_control/vtol_att_control_main.cpp | 78 | ||||
-rw-r--r-- | src/modules/vtol_att_control/vtol_att_control_params.c | 9 |
2 files changed, 85 insertions, 2 deletions
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 329e2ef94..126ec5832 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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(); @@ -353,6 +371,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. */ void @@ -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; } @@ -497,6 +541,34 @@ void VtolAttitudeControl::set_idle_mc() } 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[]) { VTOL_att_control::g_control->task_main(); @@ -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(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index b9baa2046..44157acba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -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);
\ No newline at end of file +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); + |