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

Merge pull request #1303 from PX4/launchdetectionstates 

Launchdetection improvements
This commit is contained in:
Thomas Gubler 2014-09-03 00:29:33 +02:00
parent 2780dc39ce c591444c1e
commit 6188678f77
7 changed files with 158 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -49,9 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
threshold_accel(this, "A"),
threshold_time(this, "T")
state(LAUNCHDETECTION_RES_NONE),
thresholdAccel(this, "A"),
thresholdTime(this, "T"),
motorDelay(this, "MDEL")
{
}
@ -65,34 +66,56 @@ void CatapultLaunchMethod::update(float accel_x)
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_time.get()) {
launchDetected = true;
switch (state) {
case LAUNCHDETECTION_RES_NONE:
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
if (accel_x > thresholdAccel.get()) {
integrator += dt;
if (integrator > thresholdTime.get()) {
if (motorDelay.get() > 0.0f) {
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
" throttle", (double)motorDelay.get());
} else {
/* No motor delay set: go directly to enablemotors state */
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
warnx("Launch detected: state: enablemotors (delay not activated)");
}
}
} else {
/* reset */
reset();
}
break;
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
* over to allow full throttle */
motorDelayCounter += dt;
if (motorDelayCounter > motorDelay.get()) {
warnx("Launch detected: state enablemotors");
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
break;
default:
break;
} else {
// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;
}
}
bool CatapultLaunchMethod::getLaunchDetected()
LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
{
return launchDetected;
return state;
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
motorDelayCounter = 0.0f;
state = LAUNCHDETECTION_RES_NONE;
}
}

11
src/lib/launchdetection/CatapultLaunchMethod.h
View File

@ -57,16 +57,19 @@ public:
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected() const;
void reset();
private:
hrt_abstime last_timestamp;
float integrator;
bool launchDetected;
float motorDelayCounter;
control::BlockParamFloat threshold_accel;
control::BlockParamFloat threshold_time;
LaunchDetectionResult state;
control::BlockParamFloat thresholdAccel;
control::BlockParamFloat thresholdTime;
control::BlockParamFloat motorDelay;
};

27
src/lib/launchdetection/LaunchDetector.cpp
View File

@ -46,6 +46,7 @@ namespace launchdetection
LaunchDetector::LaunchDetector() :
SuperBlock(NULL, "LAUN"),
activeLaunchDetectionMethodIndex(-1),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
void LaunchDetector::reset()
{
/* Reset all detectors */
launchMethods[0]->reset();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->reset();
}
/* Reset active launchdetector */
activeLaunchDetectionMethodIndex = -1;
}
void LaunchDetector::update(float accel_x)
@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x)
}
}
bool LaunchDetector::getLaunchDetected()
LaunchDetectionResult LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected()) {
return true;
if (activeLaunchDetectionMethodIndex < 0) {
/* None of the active launchmethods has detected a launch, check all launchmethods */
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
warnx("selecting launchmethod %d", i);
activeLaunchDetectionMethodIndex = i; // from now on only check this method
return launchMethods[i]->getLaunchDetected();
}
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
}
}
return false;
return LAUNCHDETECTION_RES_NONE;
}
}

8
src/lib/launchdetection/LaunchDetector.h
View File

@ -59,7 +59,7 @@ public:
void reset();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@ -67,6 +67,12 @@ public:
// virtual bool getLaunchDetected();
protected:
private:
int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
which detected a Launch. If no launchMethod has detected a launch yet the
value is -1. Once one launchMetthod has detected a launch only this
method is checked for further adavancing in the state machine (e.g. when
to power up the motors) */
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;

13
src/lib/launchdetection/LaunchMethod.h
View File

@ -44,11 +44,22 @@
namespace launchdetection
{
enum LaunchDetectionResult {
LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
control the attitude. However any motors should not throttle
up and still be set to 'throttlePreTakeoff'.
For instance this is used to have a delay for the motor
when launching a fixed wing aircraft from a bungee */
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
attitude and also throttle up the motors. */
};
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual LaunchDetectionResult getLaunchDetected() const = 0;
virtual void reset() = 0;
protected:

11
src/lib/launchdetection/launchdetection_params.c
View File

@ -77,6 +77,17 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
/**
* Motor delay
*
* Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller)
* Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
*
* @unit seconds
* @min 0
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
/**
* Throttle setting while detecting launch.
*

101
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */
*/
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
using namespace launchdetection;
class FixedwingPositionControl
{
public:
@ -171,8 +173,7 @@ private:
bool land_onslope;
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
LaunchDetectionResult launch_detection_state;
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
@ -438,8 +439,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
launch_detected(false),
usePreTakeoffThrust(false),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
@ -1082,39 +1082,38 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
}
} else {
/* no takeoff detection --> fly */
launch_detected = true;
warnx("launchdetection off");
if (launchDetector.launchDetectionEnabled() &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* Inform user that launchdetection is running */
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
/* update our copy of the laucn detection state */
launch_detection_state = launchDetector.getLaunchDetected();
} else {
/* no takeoff detection --> fly */
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* Set control values depending on the detection state */
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */
if (launch_detected) {
usePreTakeoffThrust = false;
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
* full throttle, otherwise we use the preTakeOff Throttle */
float takeoff_throttle = launch_detection_state !=
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
@ -1125,7 +1124,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
@ -1135,7 +1134,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
} else {
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@ -1144,17 +1144,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
usePreTakeoffThrust = true;
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f));
}
}
/* reset landing state */
@ -1188,13 +1197,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
if (usePreTakeoffThrust) {
/* Copy thrust and pitch values from tecs
* making sure again that the correct thrust is used,
* without depending on library calls */
if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@ -1348,8 +1366,7 @@ FixedwingPositionControl::task_main()
void FixedwingPositionControl::reset_takeoff_state()
{
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_state = LAUNCHDETECTION_RES_NONE;
launchDetector.reset();
}