forked from Archive/PX4-Autopilot
Merge branch 'master' into sdlog2_opt
This commit is contained in:
commit
1a795f2671
|
@ -42,7 +42,7 @@
|
|||
#include <systemlib/err.h>
|
||||
|
||||
CatapultLaunchMethod::CatapultLaunchMethod() :
|
||||
last_timestamp(0),
|
||||
last_timestamp(hrt_absolute_time()),
|
||||
integrator(0.0f),
|
||||
launchDetected(false),
|
||||
threshold_accel(NULL, "LAUN_CAT_A", false),
|
||||
|
@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams()
|
|||
threshold_accel.update();
|
||||
threshold_time.update();
|
||||
}
|
||||
|
||||
void CatapultLaunchMethod::reset()
|
||||
{
|
||||
integrator = 0.0f;
|
||||
launchDetected = false;
|
||||
}
|
||||
|
|
|
@ -55,11 +55,10 @@ public:
|
|||
void update(float accel_x);
|
||||
bool getLaunchDetected();
|
||||
void updateParams();
|
||||
void reset();
|
||||
|
||||
private:
|
||||
hrt_abstime last_timestamp;
|
||||
// float threshold_accel_raw;
|
||||
// float threshold_time;
|
||||
float integrator;
|
||||
bool launchDetected;
|
||||
|
||||
|
|
|
@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector()
|
|||
|
||||
}
|
||||
|
||||
void LaunchDetector::reset()
|
||||
{
|
||||
/* Reset all detectors */
|
||||
launchMethods[0]->reset();
|
||||
}
|
||||
|
||||
void LaunchDetector::update(float accel_x)
|
||||
{
|
||||
if (launchdetection_on.get() == 1) {
|
||||
|
|
|
@ -53,6 +53,7 @@ class __EXPORT LaunchDetector
|
|||
public:
|
||||
LaunchDetector();
|
||||
~LaunchDetector();
|
||||
void reset();
|
||||
|
||||
void update(float accel_x);
|
||||
bool getLaunchDetected();
|
||||
|
|
|
@ -47,6 +47,7 @@ public:
|
|||
virtual void update(float accel_x) = 0;
|
||||
virtual bool getLaunchDetected() = 0;
|
||||
virtual void updateParams() = 0;
|
||||
virtual void reset() = 0;
|
||||
protected:
|
||||
private:
|
||||
};
|
||||
|
|
|
@ -176,6 +176,8 @@ private:
|
|||
bool launch_detected;
|
||||
bool usePreTakeoffThrust;
|
||||
|
||||
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
|
||||
|
||||
/* Landingslope object */
|
||||
Landingslope landingslope;
|
||||
|
||||
|
@ -344,6 +346,16 @@ private:
|
|||
* Main sensor collection task.
|
||||
*/
|
||||
void task_main() __attribute__((noreturn));
|
||||
|
||||
/*
|
||||
* Reset takeoff state
|
||||
*/
|
||||
int reset_takeoff_state();
|
||||
|
||||
/*
|
||||
* Reset landing state
|
||||
*/
|
||||
int reset_landing_state();
|
||||
};
|
||||
|
||||
namespace l1_control
|
||||
|
@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
land_motor_lim(false),
|
||||
land_onslope(false),
|
||||
launch_detected(false),
|
||||
last_manual(false),
|
||||
usePreTakeoffThrust(false),
|
||||
flare_curve_alt_last(0.0f),
|
||||
launchDetector(),
|
||||
|
@ -1022,19 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
// mission is active
|
||||
_loiter_hold = false;
|
||||
|
||||
/* reset land state */
|
||||
/* reset landing state */
|
||||
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
|
||||
land_noreturn_horizontal = false;
|
||||
land_noreturn_vertical = false;
|
||||
land_stayonground = false;
|
||||
land_motor_lim = false;
|
||||
land_onslope = false;
|
||||
reset_landing_state();
|
||||
}
|
||||
|
||||
/* reset takeoff/launch state */
|
||||
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
|
||||
launch_detected = false;
|
||||
usePreTakeoffThrust = false;
|
||||
reset_takeoff_state();
|
||||
}
|
||||
|
||||
if (was_circle_mode && !_l1_control.circle_mode()) {
|
||||
|
@ -1131,6 +1139,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
|
||||
/* no flight mode applies, do not publish an attitude setpoint */
|
||||
setpoint = false;
|
||||
|
||||
/* reset landing and takeoff state */
|
||||
if (!last_manual) {
|
||||
reset_landing_state();
|
||||
reset_takeoff_state();
|
||||
}
|
||||
}
|
||||
|
||||
if (usePreTakeoffThrust) {
|
||||
|
@ -1141,6 +1155,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
}
|
||||
_att_sp.pitch_body = _tecs.get_pitch_demand();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
last_manual = false;
|
||||
} else {
|
||||
last_manual = true;
|
||||
}
|
||||
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
|
@ -1291,6 +1311,22 @@ FixedwingPositionControl::task_main()
|
|||
_exit(0);
|
||||
}
|
||||
|
||||
int FixedwingPositionControl::reset_takeoff_state()
|
||||
{
|
||||
launch_detected = false;
|
||||
usePreTakeoffThrust = false;
|
||||
launchDetector.reset();
|
||||
}
|
||||
|
||||
int FixedwingPositionControl::reset_landing_state()
|
||||
{
|
||||
land_noreturn_horizontal = false;
|
||||
land_noreturn_vertical = false;
|
||||
land_stayonground = false;
|
||||
land_motor_lim = false;
|
||||
land_onslope = false;
|
||||
}
|
||||
|
||||
int
|
||||
FixedwingPositionControl::start()
|
||||
{
|
||||
|
|
|
@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
|
|||
|
||||
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
|
||||
{
|
||||
float ewdt = w * dt;
|
||||
if (ewdt > 1.0f)
|
||||
ewdt = 1.0f; // prevent over-correcting
|
||||
ewdt *= e;
|
||||
float ewdt = e * w * dt;
|
||||
x[i] += ewdt;
|
||||
|
||||
if (i == 0) {
|
||||
|
|
|
@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
|
||||
dt = fmaxf(fminf(0.02, dt), 0.005);
|
||||
dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
|
||||
t_prev = t;
|
||||
|
||||
/* use GPS if it's valid and reference position initialized */
|
||||
|
|
Loading…
Reference in New Issue