zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add c++ style initializers where missing in EKF/common.cpp, standardize tab/space indentation, align comments and format whitespace.

main
mcsauder 3 years ago committed by Daniel Agar
parent
cab477d715
commit
a0d9687409
1 changed files with 298 additions and 298 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 154
      src/modules/ekf2/EKF/common.h

154
src/modules/ekf2/EKF/common.h
Unescape View File

@ -62,108 +62,108 @@ enum class velocity_frame_t : uint8_t { @@ -62,108 +62,108 @@ enum class velocity_frame_t : uint8_t {
};
struct gps_message {
uint64_t time_usec{0};
int32_t lat; ///< Latitude in 1E-7 degrees
int32_t lon; ///< Longitude in 1E-7 degrees
int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL
float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float yaw_offset; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
float eph; ///< GPS horizontal position accuracy in m
float epv; ///< GPS vertical position accuracy in m
float sacc; ///< GPS speed accuracy in m/s
float vel_m_s; ///< GPS ground speed (m/sec)
Vector3f vel_ned; ///< GPS ground speed NED
bool vel_ned_valid; ///< GPS ground speed is valid
uint8_t nsats; ///< number of satellites used
float pdop; ///< position dilution of precision
uint64_t time_usec{};
int32_t lat{}; ///< Latitude in 1E-7 degrees
int32_t lon{}; ///< Longitude in 1E-7 degrees
int32_t alt{}; ///< Altitude in 1E-3 meters (millimeters) above MSL
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
float eph{}; ///< GPS horizontal position accuracy in m
float epv{}; ///< GPS vertical position accuracy in m
float sacc{}; ///< GPS speed accuracy in m/s
float vel_m_s{}; ///< GPS ground speed (m/sec)
Vector3f vel_ned{}; ///< GPS ground speed NED
bool vel_ned_valid{}; ///< GPS ground speed is valid
uint8_t nsats{}; ///< number of satellites used
float pdop{}; ///< position dilution of precision
};
struct outputSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude
Vector3f vel; ///< NED velocity estimate in earth frame (m/sec)
Vector3f pos; ///< NED position estimate in earth frame (m/sec)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Quatf quat_nominal{}; ///< nominal quaternion describing vehicle attitude
Vector3f vel{}; ///< NED velocity estimate in earth frame (m/sec)
Vector3f pos{}; ///< NED position estimate in earth frame (m/sec)
};
struct outputVert {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
float vert_vel; ///< Vertical velocity calculated using alternative algorithm (m/sec)
float vert_vel_integ; ///< Integral of vertical velocity (m)
float dt; ///< delta time (sec)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float vert_vel{}; ///< Vertical velocity calculated using alternative algorithm (m/sec)
float vert_vel_integ{}; ///< Integral of vertical velocity (m)
float dt{}; ///< delta time (sec)
};
struct imuSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f delta_ang{}; ///< delta angle in body frame (integrated gyro measurements) (rad)
Vector3f delta_vel{}; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
float delta_ang_dt{0.f}; ///< delta angle integration period (sec)
float delta_vel_dt{0.f}; ///< delta velocity integration period (sec)
float delta_ang_dt{}; ///< delta angle integration period (sec)
float delta_vel_dt{}; ///< delta velocity integration period (sec)
bool delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
};
struct gpsSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Vector2f pos; ///< NE earth frame gps horizontal position measurement (m)
float hgt; ///< gps height measurement (m)
Vector3f vel; ///< NED earth frame gps velocity measurement (m/sec)
float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float hacc; ///< 1-std horizontal position error (m)
float vacc; ///< 1-std vertical position error (m)
float sacc; ///< 1-std speed error (m/sec)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector2f pos{}; ///< NE earth frame gps horizontal position measurement (m)
float hgt{}; ///< gps height measurement (m)
Vector3f vel{}; ///< NED earth frame gps velocity measurement (m/sec)
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float hacc{}; ///< 1-std horizontal position error (m)
float vacc{}; ///< 1-std vertical position error (m)
float sacc{}; ///< 1-std speed error (m/sec)
};
struct magSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Vector3f mag; ///< NED magnetometer body frame measurements (Gauss)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f mag{}; ///< NED magnetometer body frame measurements (Gauss)
};
struct baroSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
float hgt; ///< pressure altitude above sea level (m)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float hgt{}; ///< pressure altitude above sea level (m)
};
struct rangeSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
float rng; ///< range (distance to ground) measurement (m)
int8_t quality; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float rng{}; ///< range (distance to ground) measurement (m)
int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
};
struct airspeedSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
float true_airspeed; ///< true airspeed measurement (m/sec)
float eas2tas; ///< equivalent to true airspeed factor
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float true_airspeed{}; ///< true airspeed measurement (m/sec)
float eas2tas{}; ///< equivalent to true airspeed factor
};
struct flowSample {
uint64_t time_us{0}; ///< timestamp of the integration period leading edge (uSec)
Vector2f flow_xy_rad; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
Vector3f gyro_xyz; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
float dt; ///< amount of integration time (sec)
uint8_t quality; ///< quality indicator between 0 and 255
uint64_t time_us{}; ///< timestamp of the integration period leading edge (uSec)
Vector2f flow_xy_rad{}; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
Vector3f gyro_xyz{}; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
float dt{}; ///< amount of integration time (sec)
uint8_t quality{}; ///< quality indicator between 0 and 255
};
struct extVisionSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
Vector3f posVar; ///< XYZ position variances (m**2)
Matrix3f velCov; ///< XYZ velocity covariances ((m/sec)**2)
float angVar; ///< angular heading variance (rad**2)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
Quatf quat{}; ///< quaternion defining rotation from body to earth frame
Vector3f posVar{}; ///< XYZ position variances (m**2)
Matrix3f velCov{}; ///< XYZ velocity covariances ((m/sec)**2)
float angVar{}; ///< angular heading variance (rad**2)
velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
uint8_t reset_counter{0};
uint8_t reset_counter{};
};
struct dragSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Vector2f accelXY; ///< measured specific force along the X and Y body axes (m/sec**2)
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector2f accelXY{}; ///< measured specific force along the X and Y body axes (m/sec**2)
};
struct auxVelSample {
uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
Vector3f vel; ///< measured NE velocity relative to the local origin (m/sec)
Vector3f velVar; ///< estimated error variance of the NE velocity (m/sec)**2
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f vel{}; ///< measured NE velocity relative to the local origin (m/sec)
Vector3f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2
};
// Integer definitions for vdist_sensor_type
@ -247,6 +247,7 @@ struct parameters { @@ -247,6 +247,7 @@ struct parameters {
float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
float wind_vel_p_noise{1.0e-1f}; ///< process noise for wind velocity prediction (m/sec**2)
const float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
@ -331,11 +332,11 @@ struct parameters { @@ -331,11 +332,11 @@ struct parameters {
float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
// XYZ offset of sensors in body axes (m)
Vector3f imu_pos_body; ///< xyz position of IMU in body frame (m)
Vector3f gps_pos_body; ///< xyz position of the GPS antenna in body frame (m)
Vector3f rng_pos_body; ///< xyz position of range sensor in body frame (m)
Vector3f flow_pos_body; ///< xyz position of range sensor focal point in body frame (m)
Vector3f ev_pos_body; ///< xyz position of VI-sensor focal point in body frame (m)
Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
// output complementary filter tuning
float vel_Tau{0.25f}; ///< velocity state correction time constant (1/sec)
@ -358,6 +359,7 @@ struct parameters { @@ -358,6 +359,7 @@ struct parameters {
float static_pressure_coef_yp{0.0f}; // (-)
float static_pressure_coef_yn{0.0f}; // (-)
float static_pressure_coef_z{0.0f}; // (-)
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed {20.0f};
@ -388,14 +390,14 @@ struct parameters { @@ -388,14 +390,14 @@ struct parameters {
};
struct stateSample {
Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame
Vector3f vel; ///< NED velocity in earth frame in m/s
Vector3f pos; ///< NED position in earth frame in m
Vector3f delta_ang_bias; ///< delta angle bias estimate in rad
Vector3f delta_vel_bias; ///< delta velocity bias estimate in m/s
Vector3f mag_I; ///< NED earth magnetic field in gauss
Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss
Vector2f wind_vel; ///< horizontal wind velocity in earth frame in m/s
Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame
Vector3f vel{}; ///< NED velocity in earth frame in m/s
Vector3f pos{}; ///< NED position in earth frame in m
Vector3f delta_ang_bias{}; ///< delta angle bias estimate in rad
Vector3f delta_vel_bias{}; ///< delta velocity bias estimate in m/s
Vector3f mag_I{}; ///< NED earth magnetic field in gauss
Vector3f mag_B{}; ///< magnetometer bias estimate in body frame in gauss
Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s
};
union fault_status_u {
@ -420,7 +422,6 @@ union fault_status_u { @@ -420,7 +422,6 @@ union fault_status_u {
bool bad_acc_clipping : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
} flags;
uint32_t value;
};
// define structure used to communicate innovation test failures
@ -441,7 +442,6 @@ union innovation_fault_status_u { @@ -441,7 +442,6 @@ union innovation_fault_status_u {
bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
} flags;
uint16_t value;
};
// publish the status of various GPS quality checks

Write Preview
Loading…
Cancel
Save