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

FWPositionController: only store acceleration and velocity in x 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-03-08 16:33:28 +01:00
parent 28db47480d
commit b33a8686f7
2 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -367,8 +367,11 @@ FixedwingPositionControl::vehicle_attitude_poll()
_pitch = euler_angles(1);
_yaw = euler_angles(2);
_body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
_body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
_body_acceleration_x = body_acceleration(0);
Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
_body_velocity_x = body_velocity(0);
// load factor due to banking
const float load_factor = 1.f / cosf(euler_angles(0));
@ -410,7 +413,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
* by wind). Not countering this would lead to a fly-away. Only non-zero in presence
* of sufficient wind. "minimum ground speed undershoot".
*/
const float ground_speed_body = _body_velocity(0);
const float ground_speed_body = _body_velocity_x;
if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body;
@ -1414,7 +1417,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
/* Perform launch detection */
/* Detect launch using body X (forward) acceleration */
_launchDetector.update(control_interval, _body_acceleration(0));
_launchDetector.update(control_interval, _body_acceleration_x);
}
} else {
@ -2563,7 +2566,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
pitch_max_rad - radians(_param_fw_psp_off.get()),
desired_max_climbrate,
desired_max_sinkrate,
_body_acceleration(0),
_body_acceleration_x,
-_local_pos.vz,
hgt_rate_sp);

4
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
View File

@ -263,8 +263,8 @@ private:
float _yaw{0.0f};
float _yawrate{0.0f};
matrix::Vector3f _body_acceleration{};
matrix::Vector3f _body_velocity{};
float _body_acceleration_x{0.f};
float _body_velocity_x{0.f};
MapProjection _global_local_proj_ref{};
float _global_local_alt0{NAN};