fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_pos_control_l1: pass time through from run

Browse Source
This commit is contained in:
Daniel Agar 2020-08-07 21:26:56 -04:00
parent 258e5ca5c9
commit 560c22e612
9 changed files with 72 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -536,16 +536,12 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
}
bool
FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vector2f &ground_speed,
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
{
float dt = 0.01f;
if (_control_position_last_called > 0) {
dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
_control_position_last_called = hrt_absolute_time();
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
_l1_control.set_dt(dt);
@ -582,7 +578,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* save time when airplane is in air */
if (!_was_in_air && !_vehicle_land_detected.landed) {
_was_in_air = true;
_time_went_in_air = hrt_absolute_time();
_time_went_in_air = now;
_takeoff_ground_alt = _current_altitude;
}
@ -688,7 +684,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(pos_sp_curr.alt,
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
@ -745,7 +741,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
}
tecs_update_pitch_throttle(alt_sp,
tecs_update_pitch_throttle(now, alt_sp,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
@ -756,10 +752,10 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
radians(_param_fw_p_lim_min.get()));
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
control_takeoff(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
}
/* reset landing state */
@ -815,7 +811,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(_hold_alt,
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -917,7 +913,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(_hold_alt,
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -962,7 +958,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@ -1013,8 +1009,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}
void
FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
@ -1047,7 +1043,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
if (_runway_takeoff.runwayTakeoffEnabled()) {
if (!_runway_takeoff.isInitialized()) {
Eulerf euler(Quatf(_att.q));
_runway_takeoff.init(euler.psi(), _current_latitude, _current_longitude);
_runway_takeoff.init(now, euler.psi(), _current_latitude, _current_longitude);
/* need this already before takeoff is detected
* doesn't matter if it gets reset when takeoff is detected eventually */
@ -1059,7 +1055,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
// update runway takeoff helper
_runway_takeoff.update(_airspeed, _current_altitude - terrain_alt,
_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
_current_latitude, _current_longitude, &_mavlink_log_pub);
/*
@ -1071,7 +1067,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
// update tecs
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
tecs_update_pitch_throttle(pos_sp_curr.alt,
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed),
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_max_deg),
@ -1101,13 +1097,13 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
/* Perform launch detection */
/* Inform user that launchdetection is running every 4s */
if (hrt_elapsed_time(&_launch_detection_notify) > 4e6) {
if ((now - _launch_detection_notify) > 4_s) {
mavlink_log_critical(&_mavlink_log_pub, "Launch detection running");
_launch_detection_notify = hrt_absolute_time();
_launch_detection_notify = now;
}
/* Detect launch using body X (forward) acceleration */
_launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]);
_launchDetector.update(now, _vehicle_acceleration_sub.get().xyz[0]);
/* update our copy of the launch detection state */
_launch_detection_state = _launchDetector.getLaunchDetected();
@ -1142,7 +1138,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(pos_sp_curr.alt,
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
_param_fw_airspd_trim.get(),
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_max_deg),
@ -1157,7 +1153,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
} else {
tecs_update_pitch_throttle(pos_sp_curr.alt,
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -1183,8 +1179,8 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
}
void
FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
@ -1213,7 +1209,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
reset_landing_state();
_time_started_landing = hrt_absolute_time();
_time_started_landing = now;
}
const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
@ -1299,13 +1295,13 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
terrain_alt = (_local_pos.ref_alt - terrain_vpos);
_t_alt_prev_valid = terrain_alt;
_time_last_t_alt = hrt_absolute_time();
_time_last_t_alt = now;
} else if (_time_last_t_alt == 0) {
// we have started landing phase but don't have valid terrain
// wait for some time, maybe we will soon get a valid estimate
// until then just use the altitude of the landing waypoint
if (hrt_elapsed_time(&_time_started_landing) < 10_s) {
if ((now - _time_started_landing) < 10_s) {
terrain_alt = pos_sp_curr.alt;
} else {
@ -1314,7 +1310,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
abort_landing(true);
}
} else if ((!_local_pos.dist_bottom_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT)
} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
|| _land_noreturn_vertical) {
// use previous terrain estimate for some time and hope to recover
// if we are already flaring (land_noreturn_vertical) then just
@ -1372,7 +1368,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land, ground_speed),
radians(_param_fw_lnd_fl_pmin.get()),
radians(_param_fw_lnd_fl_pmax.get()),
@ -1440,7 +1436,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
tecs_update_pitch_throttle(altitude_desired,
tecs_update_pitch_throttle(now, altitude_desired,
calculate_target_airspeed(airspeed_approach, ground_speed),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -1573,8 +1569,9 @@ FixedwingPositionControl::Run()
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) {
_att_sp.timestamp = hrt_absolute_time();
if (control_position(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
_pos_sp_triplet.next)) {
// add attitude setpoint offsets
_att_sp.roll_body += radians(_param_fw_rsp_off.get());
@ -1587,15 +1584,16 @@ FixedwingPositionControl::Run()
radians(_param_fw_man_p_max.get()));
}
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
if (_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled ||
_control_mode.flag_control_altitude_enabled) {
const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
_att_sp.timestamp = hrt_absolute_time();
_attitude_sp_pub.publish(_att_sp);
// only publish status in full FW mode
@ -1650,19 +1648,14 @@ FixedwingPositionControl::reset_landing_state()
}
void
FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
uint8_t mode)
{
float dt = 0.01f; // prevent division with 0
if (_last_tecs_update > 0) {
dt = hrt_elapsed_time(&_last_tecs_update) * 1e-6;
}
_last_tecs_update = hrt_absolute_time();
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
_last_tecs_update = now;
// do not run TECS if we are not in air
bool run_tecs = !_vehicle_land_detected.landed;

11
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -325,11 +325,14 @@ private:
*/
bool update_desired_altitude(float dt);
bool control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
bool control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
void control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
void control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
void control_landing(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
float get_tecs_pitch();
@ -349,7 +352,7 @@ private:
/*
* Call TECS : a wrapper function to call the TECS implementation
*/
void tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
void tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,

6
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp
View File

@ -49,14 +49,10 @@ namespace launchdetection
CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) :
ModuleParams(parent)
{
_last_timestamp = hrt_absolute_time();
}
void CatapultLaunchMethod::update(float accel_x)
void CatapultLaunchMethod::update(const float dt, float accel_x)
{
float dt = hrt_elapsed_time(&_last_timestamp) * 1e-6f;
_last_timestamp = hrt_absolute_time();
switch (state) {
case LAUNCHDETECTION_RES_NONE:

3
src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h
View File

@ -55,13 +55,12 @@ public:
CatapultLaunchMethod(ModuleParams *parent);
~CatapultLaunchMethod() override = default;
void update(float accel_x) override;
void update(const float dt, float accel_x) override;
LaunchDetectionResult getLaunchDetected() const override;
void reset() override;
float getPitchMax(float pitchMaxDefault) override;
private:
hrt_abstime _last_timestamp{0};
float _integrator{0.0f};
float _motorDelayCounter{0.0f};

4
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp
View File

@ -68,11 +68,11 @@ void LaunchDetector::reset()
_activeLaunchDetectionMethodIndex = -1;
}
void LaunchDetector::update(float accel_x)
void LaunchDetector::update(const float dt, float accel_x)
{
if (launchDetectionEnabled()) {
for (const auto launchMethod : _launchMethods) {
launchMethod->update(accel_x);
launchMethod->update(dt, accel_x);
}
}
}

2
src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h
View File

@ -58,7 +58,7 @@ public:
void reset();
void update(float accel_x);
void update(const float dt, float accel_x);
LaunchDetectionResult getLaunchDetected();
bool launchDetectionEnabled() { return _param_laun_all_on.get(); }

2
src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h
View File

@ -59,7 +59,7 @@ class LaunchMethod
public:
virtual ~LaunchMethod() = default;
virtual void update(float accel_x) = 0;
virtual void update(const float dt, float accel_x) = 0;
virtual LaunchDetectionResult getLaunchDetected() const = 0;
virtual void reset() = 0;

22
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
View File

@ -47,6 +47,7 @@
#include <mathlib/mathlib.h>
using matrix::Vector2f;
using namespace time_literals;
namespace runwaytakeoff
{
@ -61,25 +62,25 @@ RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
{
}
void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon)
{
_init_yaw = yaw;
_initialized = true;
_state = RunwayTakeoffState::THROTTLE_RAMP;
_initialized_time = hrt_absolute_time();
_initialized_time = now;
_climbout = true; // this is true until climbout is finished
_start_wp(0) = (float)current_lat;
_start_wp(1) = (float)current_lon;
}
void RunwayTakeoff::update(float airspeed, float alt_agl,
void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
{
switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP:
if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f
|| airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f) {
if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s))
|| (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) {
_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
}
@ -191,15 +192,12 @@ float RunwayTakeoff::getYaw(float navigatorYaw)
* Ramps up in the beginning, until it lifts off the runway it is set to
* parameter value, then it returns the TECS throttle.
*/
float RunwayTakeoff::getThrottle(float tecsThrottle)
float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
{
switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP: {
float throttle = (hrt_elapsed_time(&_initialized_time) / (float)_param_rwto_ramp_time.get() * 1e6f) *
_param_rwto_max_thr.get();
return throttle < _param_rwto_max_thr.get() ?
throttle :
_param_rwto_max_thr.get();
float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
return math::min(throttle, _param_rwto_max_thr.get());
}
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:

17
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
View File

@ -68,8 +68,9 @@ public:
RunwayTakeoff(ModuleParams *parent);
~RunwayTakeoff() = default;
void init(float yaw, double current_lat, double current_lon);
void update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub);
void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon);
void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon,
orb_advert_t *mavlink_log_pub);
RunwayTakeoffState getState() { return _state; }
bool isInitialized() { return _initialized; }
@ -83,7 +84,7 @@ public:
float getPitch(float tecsPitch);
float getRoll(float navigatorRoll);
float getYaw(float navigatorYaw);
float getThrottle(float tecsThrottle);
float getThrottle(const hrt_abstime &now, float tecsThrottle);
bool resetIntegrators();
float getMinPitch(float sp_min, float climbout_min, float min);
float getMaxPitch(float max);
@ -93,11 +94,11 @@ public:
private:
/** state variables **/
RunwayTakeoffState _state;
bool _initialized;
hrt_abstime _initialized_time;
float _init_yaw;
bool _climbout;
RunwayTakeoffState _state{THROTTLE_RAMP};
bool _initialized{false};
hrt_abstime _initialized_time{0};
float _init_yaw{0.f};
bool _climbout{false};
matrix::Vector2f _start_wp;
DEFINE_PARAMETERS(