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

TECS: Use version in ECL library

Browse Source 
This change updates a number of interfaces to use the new TECS implementation from the ECL library.
This commit is contained in:
Paul Riseborough
2017-09-05 11:54:08 +10:00
committed by Lorenz Meier
parent 4923d0cba3
commit 3fc7aba178
6 changed files with 62 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+43 -46
View File
@@ -668,29 +668,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = false; // by default we don't use flaps
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
// tailsitters use the multicopter frame as reference, in fixed wing
// we need to use the fixed wing frame
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
float tmp = accel_body(0);
accel_body(0) = -accel_body(2);
accel_body(2) = tmp;
}
math::Vector<3> accel_earth{_R_nb * accel_body};
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_land_detected.landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
/* update TECS filters */
_tecs.update_state(_global_pos.alt, _airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
// l1 navigation logic breaks down when wind speed exceeds max airspeed
@@ -1474,7 +1451,7 @@ float
FixedwingPositionControl::get_tecs_pitch()
{
if (_is_tecs_running) {
return _tecs.get_pitch_demand();
return _tecs.get_pitch_setpoint();
}
// return 0 to prevent stale tecs state when it's not running
@@ -1485,7 +1462,7 @@ float
FixedwingPositionControl::get_tecs_thrust()
{
if (_is_tecs_running) {
return _tecs.get_throttle_demand();
return _tecs.get_throttle_setpoint();
}
// return 0 to prevent stale tecs state when it's not running
@@ -1514,6 +1491,7 @@ FixedwingPositionControl::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -1589,6 +1567,7 @@ FixedwingPositionControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
@@ -1821,6 +1800,28 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
pitch_for_tecs = euler(1);
}
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
// tailsitters use the multicopter frame as reference, in fixed wing
// we need to use the fixed wing frame
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
float tmp = accel_body(0);
accel_body(0) = -accel_body(2);
accel_body(2) = tmp;
}
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_land_detected.landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
/* 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.v_z_valid, _local_pos.vz, _local_pos.az);
_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
_global_pos.alt, alt_sp,
airspeed_sp, _airspeed, _eas2tas,
@@ -1828,13 +1829,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
TECS::tecs_state s {};
_tecs.get_tecs_state(s);
tecs_status_s t;
t.timestamp = _tecs.timestamp();
tecs_status_s t {};
t.timestamp = s.timestamp;
switch (s.mode) {
switch (_tecs.tecs_mode()) {
case TECS::ECL_TECS_MODE_NORMAL:
t.mode = tecs_status_s::TECS_MODE_NORMAL;
break;
@@ -1852,25 +1850,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
break;
}
t.altitudeSp = s.altitude_sp;
t.altitude_filtered = s.altitude_filtered;
t.airspeedSp = s.airspeed_sp;
t.airspeed_filtered = s.airspeed_filtered;
t.altitudeSp = _tecs.hgt_setpoint_adj();
t.altitude_filtered = _tecs.vert_pos_state();
t.airspeedSp = _tecs.TAS_setpoint_adj();
t.airspeed_filtered = _tecs.tas_state();
t.flightPathAngleSp = s.altitude_rate_sp;
t.flightPathAngle = s.altitude_rate;
t.flightPathAngleFiltered = s.altitude_rate;
t.flightPathAngleSp = _tecs.hgt_rate_setpoint();
t.flightPathAngle = _tecs.vert_vel_state();
t.airspeedDerivativeSp = s.airspeed_rate_sp;
t.airspeedDerivative = s.airspeed_rate;
t.airspeedDerivativeSp = _tecs.TAS_rate_setpoint();
t.airspeedDerivative = _tecs.speed_derivative();
t.totalEnergyError = s.total_energy_error;
t.totalEnergyRateError = s.total_energy_rate_error;
t.energyDistributionError = s.energy_distribution_error;
t.energyDistributionRateError = s.energy_distribution_rate_error;
t.totalEnergyError = _tecs.STE_error();
t.totalEnergyRateError = _tecs.STE_rate_error();
t.energyDistributionError = _tecs.SEB_error();
t.energyDistributionRateError = _tecs.SEB_rate_error();
t.throttle_integ = s.throttle_integ;
t.pitch_integ = s.pitch_integ;
t.throttle_integ = _tecs.throttle_integ_state();
t.pitch_integ = _tecs.pitch_integ_state();
if (_tecs_status_pub != nullptr) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);