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

launchdetection: rename pre takeoff throttle param and fix usage

This commit is contained in:
Thomas Gubler 2014-01-05 14:19:19 +01:00
parent d1e991f1f0
commit f387c0ccc3
4 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,7 +44,7 @@
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttle_min(NULL, "LAUN_THR_MIN", false)
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
@ -85,7 +85,7 @@ void LaunchDetector::updateParams() {
warnx(" LaunchDetector::updateParams()");
launchdetection_on.update();
throttle_min.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->updateParams();

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

@ -59,14 +59,14 @@ public:
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getMinThrottle() {return throttle_min.get(); }
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
// virtual bool getLaunchDetected();
protected:
private:
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttle_min;
control::BlockParamFloat throttlePreTakeoff;
};

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

@ -69,4 +69,4 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
// @DisplayName Throttle setting while detecting the launch
// @Description The throttle is set to this value while the system is waiting for the takeoff
// @Range 0 to 1
PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);

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

@ -174,6 +174,7 @@ private:
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
bool launch_detection_message_sent;
/* Landingslope object */
@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
launchDetector(),
launch_detected(false),
usePreTakeoffThrust(false),
launch_detection_message_sent(false)
{
/* safely initialize structs */
@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.yaw_body = _l1_control.nav_bearing();
if (launch_detected) {
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
} else {
throttle_max = launchDetector.getMinThrottle();
usePreTakeoffThrust = true;
}
}
@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_message_sent = false;
}
@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
setpoint = false;
}
if (usePreTakeoffThrust) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}