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

Remove direct unsafe access to matrix internal data

This commit is contained in:
Julian Kent 2019-09-16 10:38:17 +02:00 committed by Lorenz Meier
parent c446ee444a
commit 8d60f8ba8f
3 changed files with 12 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/ekf.h
View File

@ -227,21 +227,26 @@ public:
// return the amount the local horizontal position changed in the last reset and the number of reset events // return the amount the local horizontal position changed in the last reset and the number of reset events
void get_posNE_reset(float delta[2], uint8_t *counter) void get_posNE_reset(float delta[2], uint8_t *counter)
{ {
memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data)); delta[0] = _state_reset_status.posNE_change(0);
delta[1] = _state_reset_status.posNE_change(1);
*counter = _state_reset_status.posNE_counter; *counter = _state_reset_status.posNE_counter;
} }
// return the amount the local horizontal velocity changed in the last reset and the number of reset events // return the amount the local horizontal velocity changed in the last reset and the number of reset events
void get_velNE_reset(float delta[2], uint8_t *counter) void get_velNE_reset(float delta[2], uint8_t *counter)
{ {
memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data)); delta[0] = _state_reset_status.velNE_change(0);
delta[1] = _state_reset_status.velNE_change(1);
*counter = _state_reset_status.velNE_counter; *counter = _state_reset_status.velNE_counter;
} }
// return the amount the quaternion has changed in the last reset and the number of reset events // return the amount the quaternion has changed in the last reset and the number of reset events
void get_quat_reset(float delta_quat[4], uint8_t *counter) void get_quat_reset(float delta_quat[4], uint8_t *counter)
{ {
memcpy(delta_quat, &_state_reset_status.quat_change._data[0], sizeof(_state_reset_status.quat_change._data)); for (size_t i = 0; i < 4; i++)
{
delta_quat[i] = _state_reset_status.quat_change(i);
}
*counter = _state_reset_status.quat_counter; *counter = _state_reset_status.quat_counter;
} }

7
EKF/estimator_interface.h
View File

@ -277,13 +277,6 @@ public:
// return true if the local position estimate is valid // return true if the local position estimate is valid
bool local_position_is_valid(); bool local_position_is_valid();
void copy_quaternion(float *quat)
{
for (unsigned i = 0; i < 4; i++) {
quat[i] = _output_new.quat_nominal(i);
}
}
const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; } const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
// return the quaternion defining the rotation from the EKF to the External Vision reference frame // return the quaternion defining the rotation from the EKF to the External Vision reference frame

8
airdata/WindEstimator.cpp
View File

@ -175,7 +175,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var; const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
K /= (S._data[0][0]); K /= S(0,0);
// compute innovation // compute innovation
const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) * const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) *
(v_e - _state(w_e)) + v_d * v_d); (v_e - _state(w_e)) + v_d * v_d);
@ -183,7 +183,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
_tas_innov = true_airspeed - airspeed_pred; _tas_innov = true_airspeed - airspeed_pred;
// innovation variance // innovation variance
_tas_innov_var = S._data[0][0]; _tas_innov_var = S(0,0);
bool reinit_filter = false; bool reinit_filter = false;
bool meas_is_rejected = false; bool meas_is_rejected = false;
@ -261,7 +261,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var; const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
K /= (S._data[0][0]); K /= S(0,0);
// compute predicted side slip angle // compute predicted side slip angle
matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2)); matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2));
@ -276,7 +276,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
const float beta_pred = rel_wind(1) / rel_wind(0); const float beta_pred = rel_wind(1) / rel_wind(0);
_beta_innov = 0.0f - beta_pred; _beta_innov = 0.0f - beta_pred;
_beta_innov_var = S._data[0][0]; _beta_innov_var = S(0,0);
bool reinit_filter = false; bool reinit_filter = false;
bool meas_is_rejected = false; bool meas_is_rejected = false;