fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 13:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW use lpos

Browse Source
This commit is contained in:
Daniel Agar
2020-02-02 17:43:37 -05:00
committed by Roman Bapst
parent d303b73bc3
commit 59a7262c31
2 changed files with 72 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+65 -55
View File
@@ -54,7 +54,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
}
// limit to 50 Hz
_global_pos_sub.set_interval_ms(20);
_local_pos_sub.set_interval_ms(20);
/* fetch initial parameter values */
parameters_update();
@@ -68,8 +68,8 @@ FixedwingPositionControl::~FixedwingPositionControl()
bool
FixedwingPositionControl::init()
{
if (!_global_pos_sub.registerCallback()) {
PX4_ERR("vehicle global position callback registration failed!");
if (!_local_pos_sub.registerCallback()) {
PX4_ERR("vehicle local position callback registration failed!");
return false;
}
@@ -356,7 +356,7 @@ FixedwingPositionControl::status_publish()
pos_ctrl_status.target_bearing = _l1_control.target_bearing();
pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
@@ -405,11 +405,11 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
if (flag_init) {
// previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us
waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading + radians(180.0f),
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading + radians(180.0f),
HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon);
// next waypoint: HDG_HOLD_DIST_NEXT meters in front of us
waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading,
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading,
HDG_HOLD_DIST_NEXT, &temp_next.lat, &temp_next.lon);
} else {
@@ -434,11 +434,12 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
}
float
FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt,
const vehicle_global_position_s &global_pos)
FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt)
{
if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) {
return global_pos.terrain_alt;
float terrain_alt = _local_pos.ref_alt - (_local_pos.dist_bottom + _local_pos.z);
if (PX4_ISFINITE(terrain_alt) && _local_pos.dist_bottom_valid) {
return terrain_alt;
}
return takeoff_alt;
@@ -470,9 +471,9 @@ FixedwingPositionControl::update_desired_altitude(float dt)
* when the altitude certainty increases or decreases.
*/
if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
if (fabsf(_althold_epv - _local_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
_hold_alt = _current_altitude;
_althold_epv = _local_pos.epv;
}
/*
@@ -497,14 +498,14 @@ FixedwingPositionControl::update_desired_altitude(float dt)
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
_hold_alt = _current_altitude;
_althold_epv = _local_pos.epv;
_was_in_deadband = true;
}
if (_vehicle_status.is_vtol) {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
_hold_alt = _global_pos.alt;
_hold_alt = _current_altitude;
}
}
@@ -520,10 +521,8 @@ FixedwingPositionControl::in_takeoff_situation()
}
// in air for < 10s
const hrt_abstime delta_takeoff = 10_s;
return (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff)
&& (_global_pos.alt <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
return (hrt_elapsed_time(&_time_went_in_air) < 10_s)
&& (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
}
void
@@ -584,7 +583,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
if (!_was_in_air && !_vehicle_land_detected.landed) {
_was_in_air = true;
_time_went_in_air = hrt_absolute_time();
_takeoff_ground_alt = _global_pos.alt;
_takeoff_ground_alt = _current_altitude;
}
/* reset flag when airplane landed */
@@ -604,7 +603,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_control_mode_current = FW_POSCTRL_MODE_AUTO;
/* reset hold altitude */
_hold_alt = _global_pos.alt;
_hold_alt = _current_altitude;
/* reset hold yaw */
_hdg_hold_yaw = _yaw;
@@ -712,7 +711,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}
if (_land_abort) {
if (pos_sp_curr.alt - _global_pos.alt < _param_fw_clmbout_diff.get()) {
if (pos_sp_curr.alt - _current_altitude < _param_fw_clmbout_diff.get()) {
// aborted landing complete, normal loiter over landing point
abort_landing(false);
@@ -763,7 +762,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
@@ -835,7 +834,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}
/* we have a valid heading hold position, are we too close? */
float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _hdg_hold_curr_wp.lat,
float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat,
_hdg_hold_curr_wp.lon);
if (dist < HDG_HOLD_REACHED_DIST) {
@@ -872,7 +871,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
@@ -917,7 +916,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
setpoint = false;
// reset hold altitude
_hold_alt = _global_pos.alt;
_hold_alt = _current_altitude;
/* reset landing and takeoff state */
if (!_last_manual) {
@@ -1026,20 +1025,20 @@ 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(), _global_pos.lat, _global_pos.lon);
_runway_takeoff.init(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 */
_takeoff_ground_alt = _global_pos.alt;
_takeoff_ground_alt = _current_altitude;
mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway");
}
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
// update runway takeoff helper
_runway_takeoff.update(_airspeed, _global_pos.alt - terrain_alt,
_global_pos.lat, _global_pos.lon, &_mavlink_log_pub);
_runway_takeoff.update(_airspeed, _current_altitude - terrain_alt,
_current_latitude, _current_longitude, &_mavlink_log_pub);
/*
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1116,7 +1115,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());
const float altitude_error = pos_sp_curr.alt - _global_pos.alt;
const float altitude_error = pos_sp_curr.alt - _current_altitude;
/* 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()) {
@@ -1273,9 +1272,10 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
float terrain_alt = pos_sp_curr.alt;
if (_param_fw_lnd_useter.get() == 1) {
if (_global_pos.terrain_alt_valid) {
if (_local_pos.dist_bottom_valid) {
// all good, have valid terrain altitude
terrain_alt = _global_pos.terrain_alt;
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();
@@ -1292,7 +1292,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
abort_landing(true);
}
} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT)
} else if ((!_local_pos.dist_bottom_valid && hrt_elapsed_time(&_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
@@ -1310,7 +1310,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if ((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) ||
if ((_current_altitude < terrain_alt + _landingslope.flare_relative_alt()) ||
_land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
_att_sp.fw_control_yaw = true;
}
if (((_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt()) &&
if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) &&
(wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP
_land_motor_lim) {
throttle_max = min(throttle_max, _param_fw_thr_lnd_max.get());
@@ -1364,14 +1364,14 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
if (!_land_noreturn_vertical) {
// just started with the flaring phase
_flare_pitch_sp = 0.0f;
_flare_height = _global_pos.alt - terrain_alt;
_flare_height = _current_altitude - terrain_alt;
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
_land_noreturn_vertical = true;
} else {
if (_local_pos.vz > 0.1f) {
_flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) *
constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
constrain((_flare_height - (_current_altitude - terrain_alt)) / _flare_height, 0.0f, 1.0f);
}
// otherwise continue using previous _flare_pitch_sp
@@ -1397,7 +1397,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
const float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp);
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
if (_current_altitude > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
/* stay on slope */
altitude_desired = terrain_alt + landing_slope_alt_rel_desired;
@@ -1412,7 +1412,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
altitude_desired = pos_sp_prev.alt;
} else {
altitude_desired = _global_pos.alt;
altitude_desired = _current_altitude;
}
}
@@ -1470,7 +1470,7 @@ void
FixedwingPositionControl::Run()
{
if (should_exit()) {
_global_pos_sub.unregisterCallback();
_local_pos_sub.unregisterCallback();
exit_and_cleanup();
return;
}
@@ -1478,7 +1478,10 @@ FixedwingPositionControl::Run()
perf_begin(_loop_perf);
/* only run controller if position changed */
if (_global_pos_sub.update(&_global_pos)) {
float lpos_x_prev = _local_pos.x;
float lpos_y_prev = _local_pos.y;
if (_local_pos_sub.update(&_local_pos)) {
// check for parameter updates
if (_parameter_update_sub.updated()) {
@@ -1490,19 +1493,26 @@ FixedwingPositionControl::Run()
parameters_update();
}
_local_pos_sub.update(&_local_pos);
vehicle_global_position_s gpos;
if (_global_pos_sub.update(&gpos)) {
_current_latitude = gpos.lat;
_current_longitude = gpos.lon;
}
_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) {
_hold_alt += _global_pos.delta_alt;
if (_control_mode.flag_control_altitude_enabled && _current_altitude_reset_counter != _alt_reset_counter) {
_hold_alt += -_local_pos.delta_z;
// make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt);
_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
}
// adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
&& _current_latitude_lon_reset_counter != _pos_reset_counter) {
// reset heading hold flag, which will re-initialise position control
_hdg_hold_enabled = false;
@@ -1510,8 +1520,8 @@ FixedwingPositionControl::Run()
}
// update the reset counters in any case
_alt_reset_counter = _global_pos.alt_reset_counter;
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
_alt_reset_counter = _current_altitude_reset_counter;
_pos_reset_counter = _current_latitude_lon_reset_counter;
airspeed_poll();
_manual_control_sub.update(&_manual);
@@ -1524,7 +1534,7 @@ FixedwingPositionControl::Run()
_vehicle_acceleration_sub.update();
_vehicle_rates_sub.update();
Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
Vector2f curr_pos((float)_current_latitude, (float)_current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
//Convert Local setpoints to global setpoints
@@ -1718,8 +1728,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* update TECS vehicle state estimates */
_tecs.update_vehicle_state_estimates(_airspeed, _R_nb,
accel_body, (_global_pos.timestamp > 0), in_air_alt_control,
_global_pos.alt, _local_pos.vz);
accel_body, (_local_pos.timestamp > 0), in_air_alt_control,
_current_altitude, _local_pos.vz);
/* scale throttle cruise by baro pressure */
if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
@@ -1738,7 +1748,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
}
_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
_global_pos.alt, alt_sp,
_current_altitude, alt_sp,
airspeed_sp, _airspeed, _eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+7 -4
View File
@@ -148,10 +148,10 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
uORB::SubscriptionCallbackWorkItem _global_pos_sub{this, ORB_ID(vehicle_global_position)};
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
@@ -173,7 +173,6 @@ private:
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
vehicle_command_s _vehicle_command {}; ///< vehicle commands
vehicle_control_mode_s _control_mode {}; ///< control mode
vehicle_global_position_s _global_pos {}; ///< global vehicle position
vehicle_local_position_s _local_pos {}; ///< vehicle local position
vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected
vehicle_status_s _vehicle_status {}; ///< vehicle status
@@ -181,6 +180,10 @@ private:
SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
double _current_latitude{0};
double _current_longitude{0};
float _current_altitude{0.f};
perf_counter_t _loop_perf; ///< loop performance counter
float _hold_alt{0.0f}; ///< hold altitude for altitude mode
@@ -298,7 +301,7 @@ private:
/**
* Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
*/
float get_terrain_altitude_takeoff(float takeoff_alt, const vehicle_global_position_s &global_pos);
float get_terrain_altitude_takeoff(float takeoff_alt);
/**
* Check if we are in a takeoff situation