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

AirspeedSelector: use Vector3f

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-12 20:14:30 +02:00
parent 259b851ba7
commit ccab93e68b
3 changed files with 13 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -50,12 +50,11 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
// get indicated airspeed from input data (raw airspeed)
_IAS = input_data.airspeed_indicated_raw;
update_CAS_scale_estimated(input_data.lpos_valid, input_data.lpos_vx, input_data.lpos_vy, input_data.lpos_vz);
update_CAS_scale_estimated(input_data.lpos_valid, input_data.ground_velocity);
update_CAS_scale_applied();
update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
input_data.lpos_vy,
input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
check_airspeed_data_stuck(input_data.timestamp);
check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
@ -72,14 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
void
AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
float lpos_vx, float lpos_vy,
float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
matrix::Vector3f vI, float lpos_evh, float lpos_evv, const float att_q[4])
{
_wind_estimator.update(time_now_usec);
if (lpos_valid && _in_fixed_wing_flight) {
Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
Quatf q(att_q);
// airspeed fusion (with raw TAS)
@ -117,7 +114,7 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
}
void
AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz)
AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI)
{
if (!_in_fixed_wing_flight) {
return;
@ -128,10 +125,10 @@ AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float v
reset_CAS_scale_check();
}
const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx));
const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vI(0), vI(1)));
const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F));
_scale_check_groundspeed(segment_index) = sqrt(vx * vx + vy * vy + vz * vz);
_scale_check_groundspeed(segment_index) = vI.norm();
_scale_check_TAS(segment_index) = _TAS;
// run check if all segments are filled

10
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -55,9 +55,7 @@ struct airspeed_validator_update_data {
float airspeed_indicated_raw;
float airspeed_true_raw;
uint64_t airspeed_timestamp;
float lpos_vx;
float lpos_vy;
float lpos_vz;
matrix::Vector3f ground_velocity;
bool lpos_valid;
float lpos_evh;
float lpos_evv;
@ -173,11 +171,9 @@ private:
void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
float lpos_vy,
float lpos_vz,
void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, matrix::Vector3f vI,
float lpos_evh, float lpos_evv, const float att_q[4]);
void update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz);
void update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI);
void update_CAS_scale_applied();
void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
void check_airspeed_data_stuck(uint64_t timestamp);

6
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -329,12 +329,12 @@ AirspeedModule::Run()
_position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
const matrix::Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
// Prepare data for airspeed_validator
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = _time_now_usec;
input_data.lpos_vx = _vehicle_local_position.vx;
input_data.lpos_vy = _vehicle_local_position.vy;
input_data.lpos_vz = _vehicle_local_position.vz;
input_data.ground_velocity = vI;
input_data.lpos_valid = _vehicle_local_position_valid;
input_data.lpos_evh = _vehicle_local_position.evh;
input_data.lpos_evv = _vehicle_local_position.evv;