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

Adaptive quadchute

This commit is contained in:
sanderux 2017-10-17 15:42:45 +02:00 committed by Lorenz Meier
parent 9c8dc3941d
commit fc16a31f89
5 changed files with 64 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -132,6 +132,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_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");
_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
@ -364,6 +365,22 @@ VtolAttitudeControl::vehicle_local_pos_poll()
}
/**
* Check for setpoint updates.
*/
void
VtolAttitudeControl::vehicle_local_pos_sp_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_local_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp);
}
}
/**
* Check for position setpoint updates.
*/
@ -571,6 +588,10 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.fw_min_alt, &v);
_params.fw_min_alt = v;
/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
param_get(_params_handles.fw_alt_err, &v);
_params.fw_alt_err = v;
/* maximum pitch angle (QuadChute) */
param_get(_params_handles.fw_qc_max_pitch, &l);
_params.fw_qc_max_pitch = l;
@ -666,6 +687,7 @@ void VtolAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_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));
@ -754,6 +776,7 @@ void VtolAttitudeControl::task_main()
vehicle_rates_sp_fw_poll();
parameters_update_poll();
vehicle_local_pos_poll(); // Check for new sensor values
vehicle_local_pos_sp_poll();
pos_sp_triplet_poll();
vehicle_airspeed_poll();
vehicle_battery_poll();

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

@ -75,6 +75,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vtol_vehicle_status.h>
@ -114,6 +115,7 @@ public:
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 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;}
@ -140,6 +142,7 @@ private:
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
@ -173,6 +176,7 @@ 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 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
@ -196,6 +200,7 @@ private:
param_t vtol_type;
param_t elevons_mc_lock;
param_t fw_min_alt;
param_t fw_alt_err;
param_t fw_qc_max_pitch;
param_t fw_qc_max_roll;
param_t front_trans_time_openloop;
@ -227,6 +232,7 @@ private:
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 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.

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

@ -308,6 +308,16 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
*/
PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
/**
* Adaptive QuadChute
*
* Maximum negative altitude error, when in fixed wing the altitude drops below this copared to the altitude setpoint
* the vehicle will transition back to MC mode and enter failsafe RTL
* @min 0.0
* @max 200.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
/**
* QuadChute Max Pitch

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

@ -65,6 +65,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_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();
@ -220,7 +221,28 @@ void VtolType::check_quadchute_condition()
if (_params->fw_min_alt > FLT_EPSILON) {
if (-(_local_pos->z) < _params->fw_min_alt) {
_attc->abort_front_transition("Minimum altitude breached");
_attc->abort_front_transition("QuadChute: Minimum altitude breached");
}
}
// adaptive quadchute
// We use tecs for tracking in FW and local_pos_sp during transitions
if (_params->fw_alt_err > FLT_EPSILON) {
float altErr = 0.0f;
if (_tecs_running) {
altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered;
} else {
altErr = -_local_pos_sp->z - -_local_pos->z;
if (!_local_pos->z_valid) {
altErr = 0.0f;
}
}
if (altErr > _params->fw_alt_err) {
_attc->abort_front_transition("QuadChute: Altitude error too large");
}
}

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

@ -60,6 +60,7 @@ struct Params {
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)
float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute)
float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute)
float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute)
float front_trans_time_openloop;
@ -168,6 +169,7 @@ protected:
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 airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct tecs_status_s *_tecs_status;