fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 08:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improve naming and white space

Browse Source
This commit is contained in:
kamilritz
2020-01-08 09:00:31 +01:00
committed by Mathieu Bresciani
parent db73a06620
commit 2657e4fc0c
3 changed files with 21 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+8 -8
View File
@@ -208,10 +208,10 @@ void Ekf::controlExternalVisionFusion()
}
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
&& !_ev_rot_mat_initialised) {
&& !_R_ev_to_ekf_initialised) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_ev_rot_mat_initialised = true;
_R_ev_to_ekf_initialised = true;
ECL_INFO_TIMESTAMPED("external vision aligned");
}
}
@@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion()
Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
// rotate measurement into body frame is required when fusing with GPS
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
ev_delta_pos = _R_ev_to_ekf * ev_delta_pos;
// use the change in position since the last measurement
_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
@@ -319,7 +319,7 @@ void Ekf::controlExternalVisionFusion()
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
}
_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
@@ -353,7 +353,7 @@ void Ekf::controlExternalVisionFusion()
// rotate measurement into correct earth frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel;
vel_aligned = _R_ev_to_ekf * _ev_sample_delayed.vel;
}
// correct velocity for offset relative to IMU
@@ -807,7 +807,7 @@ void Ekf::controlHeightSensorTimeouts()
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
const bool reset_to_gps = !_gps_hgt_intermittent &&
( (gps_hgt_accurate && !prev_bad_vert_accel) || !baro_data_available );
((gps_hgt_accurate && !prev_bad_vert_accel) || !baro_data_available);
if (reset_to_gps) {
// set height sensor health
@@ -844,8 +844,8 @@ void Ekf::controlHeightSensorTimeouts()
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
const bool reset_to_baro = baro_data_fresh &&
( (baro_data_consistent && !_baro_hgt_faulty && !gps_hgt_accurate) ||
_gps_hgt_intermittent );
((baro_data_consistent && !_baro_hgt_faulty && !gps_hgt_accurate) ||
_gps_hgt_intermittent);
if (reset_to_baro) {
// set height sensor health
EKF/ekf.h
+2 -2
View File
@@ -322,9 +322,9 @@ private:
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
AxisAnglef _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad)
Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec)
bool _ev_rot_mat_initialised{0}; ///< _ev_rot_mat should only be initialised once in the beginning through the reset function
bool _R_ev_to_ekf_initialised{0}; ///< _R_ev_to_ekf should only be initialised once in the beginning through the reset function
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
EKF/ekf_helper.cpp
+11 -11
View File
@@ -97,7 +97,7 @@ bool Ekf::resetVelocity()
ECL_INFO_TIMESTAMPED("reset velocity to ev velocity");
Vector3f _ev_vel = _ev_sample_delayed.vel;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _ev_rot_mat *_ev_sample_delayed.vel;
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
_state.vel = _ev_vel;
P.uncorrelateCovarianceSetVariance<3>(4, sq(_ev_sample_delayed.velErr));
@@ -159,7 +159,7 @@ bool Ekf::resetPosition()
// this reset is only called if we have new ev data at the fusion time horizon
Vector3f _ev_pos = _ev_sample_delayed.pos;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_pos = _ev_rot_mat *_ev_sample_delayed.pos;
_ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos;
}
_state.pos(0) = _ev_pos(0);
_state.pos(1) = _ev_pos(1);
@@ -403,10 +403,10 @@ bool Ekf::realignYawGPS()
const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// calculate course yaw angle
const float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
// Check the EKF and GPS course over ground for consistency
const float courseYawError = gpsCOG - ekfGOG;
const float courseYawError = gpsCOG - ekfCOG;
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
const bool badYawErr = fabsf(courseYawError) > 0.5f;
@@ -629,7 +629,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// calculate the observed yaw angle
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
@@ -686,7 +686,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
const Quatf q_error( (quat_after_reset * quat_before_reset.inversed()).normalized() );
const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
// update quaternion states
_state.quat_nominal = quat_after_reset;
@@ -790,9 +790,9 @@ void Ekf::constrainStates()
// calculate the earth rotation vector
Vector3f Ekf::calcEarthRateNED(float lat_rad) const
{
return Vector3f{CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
0.0f,
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)};
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
}
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
@@ -1634,7 +1634,7 @@ void Ekf::startMag3DFusion()
void Ekf::calcExtVisRotMat()
{
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
const Quatf q_error( (_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized() );
const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
// convert to a delta angle and apply a spike and low pass filter
AxisAnglef rot_vec(q_error);
@@ -1657,7 +1657,7 @@ void Ekf::calcExtVisRotMat()
}
_ev_rot_mat = Dcmf(_ev_rot_vec_filt); // rotation from EV reference to EKF reference
_R_ev_to_ekf = Dcmf(_ev_rot_vec_filt);
}
@@ -1680,7 +1680,7 @@ void Ekf::resetExtVisRotMat()
_ev_rot_vec_filt.zero();
}
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
_R_ev_to_ekf = Dcmf(q_error);
}
// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame