AttPosEKF: Move documentation to header file

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();

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)
 {
 

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);