diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index eabef2798f..fb88fa8e81 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -332,13 +332,13 @@ private: namespace estimator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; + /* oddly, ERROR is not defined for c++ */ + #ifdef ERROR + # undef ERROR + #endif + static const int ERROR = -1; -AttitudePositionEstimatorEKF *g_estimator = nullptr; + AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : @@ -525,16 +525,14 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() estimator::g_estimator = nullptr; } -int -AttitudePositionEstimatorEKF::enable_logging(bool logging) +int AttitudePositionEstimatorEKF::enable_logging(bool logging) { _ekf_logging = logging; return 0; } -int -AttitudePositionEstimatorEKF::parameters_update() +int AttitudePositionEstimatorEKF::parameters_update() { param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); @@ -578,8 +576,7 @@ AttitudePositionEstimatorEKF::parameters_update() return OK; } -void -AttitudePositionEstimatorEKF::vehicle_status_poll() +void AttitudePositionEstimatorEKF::vehicle_status_poll() { bool vstatus_updated; @@ -592,8 +589,7 @@ AttitudePositionEstimatorEKF::vehicle_status_poll() } } -int -AttitudePositionEstimatorEKF::check_filter_state() +int AttitudePositionEstimatorEKF::check_filter_state() { /* * CHECK IF THE INPUT DATA IS SANE @@ -709,14 +705,12 @@ AttitudePositionEstimatorEKF::check_filter_state() return check; } -void -AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) +void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[]) { estimator::g_estimator->task_main(); } -void -AttitudePositionEstimatorEKF::task_main() +void AttitudePositionEstimatorEKF::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1637,8 +1631,7 @@ AttitudePositionEstimatorEKF::task_main() _exit(0); } -int -AttitudePositionEstimatorEKF::start() +int AttitudePositionEstimatorEKF::start() { ASSERT(_estimator_task == -1); @@ -1658,8 +1651,7 @@ AttitudePositionEstimatorEKF::start() return OK; } -void -AttitudePositionEstimatorEKF::print_status() +void AttitudePositionEstimatorEKF::print_status() { math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 4e4ef3b927..28d0fde34f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2049,11 +2049,6 @@ void AttPosEKF::FuseOptFlow() } } -/* -Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF -This fiter requires optical flow rates that are not motion compensated -Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser -*/ void AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption @@ -2806,12 +2801,6 @@ bool AttPosEKF::FilterHealthy() return true; } -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ void AttPosEKF::ResetPosition(void) { if (staticMode) { @@ -2825,20 +2814,12 @@ void AttPosEKF::ResetPosition(void) } } -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; } -/** - * Reset the velocity state. - */ void AttPosEKF::ResetVelocity(void) { if (staticMode) { @@ -2931,15 +2912,6 @@ out: } -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 898105db48..be382127b3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -230,6 +230,12 @@ public: void FuseOptFlow(); + /** + * @brief + * Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF + * This fiter requires optical flow rates that are not motion compensated + * Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser + **/ void OpticalFlowEKF(); void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last); @@ -290,10 +296,31 @@ public: void ForceSymmetry(); + /** + * @brief + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ int CheckAndBound(struct ekf_status_report *last_error); + /** + * @brief + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ void ResetPosition(); + /** + * @brief + * Reset the velocity state. + */ void ResetVelocity(); void ZeroVariables(); @@ -309,7 +336,8 @@ public: protected: /** - * @brief Initializes algorithm parameters to safe default values + * @brief + * Initializes algorithm parameters to safe default values **/ void InitialiseParameters(); @@ -321,7 +349,13 @@ protected: bool VelNEDDiverged(); - void ResetHeight(void); + /** + * @brief + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ + void ResetHeight(); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);