AttPosEKF: Subscribe to vehicle armed status

2 files changed, 15 insertions, 9 deletions

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 228ffa853..ec9efe8ee 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -143,6 +143,7 @@ private:
     int     _mission_sub;
     int     _home_sub;          /**< home position as defined by commander / user */
     int     _landDetectorSub;
+    int     _armedSub;
 
     orb_advert_t    _att_pub;           /**< vehicle attitude */
     orb_advert_t    _global_pos_pub;        /**< global position */
@@ -163,6 +164,7 @@ private:
     struct wind_estimate_s              _wind;          /**< wind estimate */
     struct range_finder_report          _distance;      /**< distance estimate */
     struct vehicle_land_detected_s      _landDetector;
+    struct actuator_armed_s             _armed;
 
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];
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 903158129..7de44c75b 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
@@ -122,6 +122,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mission_sub(-1),
 	_home_sub(-1),
 	_landDetectorSub(-1),
+	_armedSub(-1),
 
 	/* publications */
 	_att_pub(-1),
@@ -143,6 +144,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_wind({}),
 	_distance{},
 	_landDetector{},
+	_armed{},
 
 	_gyro_offsets({}),
 	_accel_offsets({}),
@@ -334,7 +336,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
 		_ekf->posDSigma = _parameters.posd_noise;
 		_ekf->magMeasurementSigma = _parameters.mag_noise;
 		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
-        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+        _ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
 		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
 	}
@@ -497,6 +499,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
 	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -1096,11 +1099,18 @@ void AttitudePositionEstimatorEKF::print_status()
 
 void AttitudePositionEstimatorEKF::pollData()
 {
+	//Update arming status
+	bool armedUpdate;
+	orb_check(_armedSub, &armedUpdate);
+	if(armedUpdate) {
+		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
+	}
+
+	//Update Gyro and Accelerometer
 	static Vector3f lastAngRate;
 	static Vector3f lastAccel;
 	bool accel_updated = false;
 
-	//Update Gyro and Accelerometer
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
 	static hrt_abstime last_accel = 0;
@@ -1413,12 +1423,7 @@ int AttitudePositionEstimatorEKF::trip_nan() {
 	int ret = 0;
 
 	// If system is not armed, inject a NaN value into the filter
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
-	struct actuator_armed_s armed;
-	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
-	if (armed.armed) {
+	if (_armed.armed) {
 		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
 		ret = 1;
 	} else {
@@ -1453,7 +1458,6 @@ int AttitudePositionEstimatorEKF::trip_nan() {
 		print_status();
 	}
 
-	close(armed_sub);
 	return ret;
 }