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

TECS: Use version in ECL library 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/tecs_status.msg
View File

@ -11,7 +11,6 @@ float32 altitudeSp
float32 altitude_filtered
float32 flightPathAngleSp
float32 flightPathAngle
float32 flightPathAngleFiltered
float32 airspeedSp
float32 airspeed_filtered
float32 airspeedDerivativeSp

4
msg/vehicle_local_position.msg
View File

@ -29,6 +29,10 @@ uint8 vxy_reset_counter
float32 delta_vz
uint8 vz_reset_counter
# Acceleration in NED frame
float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2)
float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
# Heading
float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)

2
src/lib/ecl

@ -1 +1 @@
Subproject commit 2bae55753dc3cb83ae8bd92be5dcc76f27cf8948
Subproject commit f640dbcb538c9d033385ad454fd3288cfd0efd4f

7
src/modules/ekf2/ekf2_main.cpp
View File

@ -904,6 +904,13 @@ void Ekf2::run()
_ekf.get_pos_d_deriv(&pos_d_deriv);
lpos.z_deriv = pos_d_deriv; // vertical position time derivative (m/s)
// Acceleration of body origin in local NED frame
float vel_deriv[3] = {};
_ekf.get_vel_deriv_ned(vel_deriv);
lpos.ax = vel_deriv[0];
lpos.ay = vel_deriv[1];
lpos.az = vel_deriv[2];
// TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail;
lpos.z_valid = !_vel_innov_preflt_fail;

89
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
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);

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

@ -33,21 +33,14 @@
/**
* @file fw_pos_control_l1_main.c
* @file fw_pos_control_l1_main.hpp
* Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll
* angle, equivalent to a lateral motion (for copters and rovers).
*
* Original publication for horizontal control class:
* S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
* The implementation for the controllers is in the ECL library. This class only
* interfaces to the library.
*
* Original implementation for total energy control class:
* Paul Riseborough, ECL Library, 2017
*
* More details and acknowledgements in the referenced library headers.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*/
@ -85,6 +78,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
@ -152,6 +146,7 @@ private:
bool _task_running{false}; ///< if true, task is running in its mainloop */
int _global_pos_sub{-1};
int _local_pos_sub{-1};
int _pos_sp_triplet_sub{-1};
int _control_mode_sub{-1}; ///< control mode subscription */
int _vehicle_attitude_sub{-1}; ///< vehicle attitude subscription */
@ -175,6 +170,7 @@ private:
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 */