ekf2: Improved publishing rules

If the ekf has not completed alignment or encounters a serious error that produces NaN's on the attitude states, then the control, attitude and position topics are not published
The control topic is published first to reduce latency

src/modules/ekf2/ekf2_main.cpp

@@ -377,20 +377,22 @@ void Ekf2::task_main()
 
 		orb_check(_optical_flow_sub, &optical_flow_updated);
 
-		if(optical_flow_updated) {
+		if (optical_flow_updated) {
 			orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow);
 		}
 
 		orb_check(_range_finder_sub, &range_finder_updated);
 
-		if(range_finder_updated) {
+		if (range_finder_updated) {
 			orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
 		}
 
 		// in replay mode we are getting the actual timestamp from the sensor topic
 		hrt_abstime now = 0;
+
 		if (_replay_mode) {
 			now = sensors.timestamp;
+
 		} else {
 			now = hrt_absolute_time();
 		}
@@ -434,7 +436,7 @@ void Ekf2::task_main()
 			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
 		}
 
-		if(optical_flow_updated) {
+		if (optical_flow_updated) {
 			flow_message flow;
 			flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
 			flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
@@ -442,12 +444,13 @@ void Ekf2::task_main()
 			flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
 			flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
 			flow.dt = optical_flow.integration_timespan;
-			if(!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
+
+			if (!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) {
 				_ekf->setOpticalFlowData(optical_flow.timestamp, &flow);
 			}
 		}
 
-		if(range_finder_updated) {
+		if (range_finder_updated) {
 			_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}
 
@@ -461,20 +464,60 @@ void Ekf2::task_main()
 			_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
 		}
 
-		// run the EKF update
-		_ekf->update();
+		// run the EKF update and output
+		if (_ekf->update()) {
 
-		// generate vehicle attitude data
+			// generate vehicle attitude quaternion data
 			struct vehicle_attitude_s att = {};
-		att.timestamp = hrt_absolute_time();
-
 			_ekf->copy_quaternion(att.q);
 			matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
+
+			// generate control state data
+			control_state_s ctrl_state = {};
+			ctrl_state.timestamp = hrt_absolute_time();
+			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
+
+			ctrl_state.q[0] = q(0);
+			ctrl_state.q[1] = q(1);
+			ctrl_state.q[2] = q(2);
+			ctrl_state.q[3] = q(3);
+
+			// publish control state data
+			if (_control_state_pub == nullptr) {
+				_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+
+			} else {
+				orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
+			}
+
+
+			// generate remaining vehicle attitude data
+			att.timestamp = hrt_absolute_time();
 			matrix::Euler<float> euler(q);
 			att.roll = euler(0);
 			att.pitch = euler(1);
 			att.yaw = euler(2);
 
+			att.q[0] = q(0);
+			att.q[1] = q(1);
+			att.q[2] = q(2);
+			att.q[3] = q(3);
+			att.q_valid = true;
+
+			att.rollspeed = sensors.gyro_rad_s[0];
+			att.pitchspeed = sensors.gyro_rad_s[1];
+			att.yawspeed = sensors.gyro_rad_s[2];
+
+			// publish vehicle attitude data
+			if (_att_pub == nullptr) {
+				_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
+			}
+
 			// generate vehicle local position data
 			struct vehicle_local_position_s lpos = {};
 			float pos[3] = {};
@@ -502,7 +545,8 @@ void Ekf2::task_main()
 
 			// Position of local NED origin in GPS / WGS84 frame
 			struct map_projection_reference_s ekf_origin = {};
-		_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); 	// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+			// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+			_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
 			lpos.xy_global = _ekf->global_position_is_valid();
 			lpos.z_global = true;                                // true if z is valid and has valid global reference (ref_alt)
 			lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
@@ -517,13 +561,11 @@ void Ekf2::task_main()
 			lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate
 			lpos.surface_bottom_timestamp	= hrt_absolute_time(); // Time when new bottom surface found
 
-		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
-		// TODO: Should use sqrt of filter position variances
-		// get pos vel state variance
+			// TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
 			Vector3f pos_var, vel_var;
 			_ekf->get_pos_var(pos_var);
 			_ekf->get_vel_var(vel_var);
-		lpos.eph = sqrt(pos_var(0)+pos_var(1));
+			lpos.eph = sqrt(pos_var(0) + pos_var(1));
 			lpos.epv = sqrt(pos_var(2));
 
 			// publish vehicle local position data
@@ -534,51 +576,10 @@ void Ekf2::task_main()
 				orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
 			}
 
-		// generate control state data
-		control_state_s ctrl_state = {};
-		ctrl_state.timestamp = hrt_absolute_time();
-		ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
-		ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
-		ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
-
-		ctrl_state.q[0] = q(0);
-		ctrl_state.q[1] = q(1);
-		ctrl_state.q[2] = q(2);
-		ctrl_state.q[3] = q(3);
-
-		// publish control state data
-		if (_control_state_pub == nullptr) {
-			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
-
-		} else {
-			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
-		}
-
-		// generate vehicle attitude data
-		att.q[0] = q(0);
-		att.q[1] = q(1);
-		att.q[2] = q(2);
-		att.q[3] = q(3);
-		att.q_valid = true;
-
-		att.rollspeed = sensors.gyro_rad_s[0];
-		att.pitchspeed = sensors.gyro_rad_s[1];
-		att.yawspeed = sensors.gyro_rad_s[2];
-
-		// publish vehicle attitude data
-		if (_att_pub == nullptr) {
-			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
-		}
-
 			// generate and publish global position data
 			struct vehicle_global_position_s global_pos = {};
 
 			if (_ekf->global_position_is_valid()) {
-			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
-
 				global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
 				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
 
@@ -595,7 +596,7 @@ void Ekf2::task_main()
 
 				global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
 
-			global_pos.eph = sqrt(pos_var(0)+pos_var(1));; // Standard deviation of position estimate horizontally
+				global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
 				global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
 
 				// TODO: implement terrain estimator
@@ -613,6 +614,7 @@ void Ekf2::task_main()
 					orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
 				}
 			}
+		}
 
 		// publish estimator status
 		struct estimator_status_s status = {};
@@ -642,6 +644,7 @@ void Ekf2::task_main()
 		_ekf->get_heading_innov_var(&innovations.heading_innov_var);
 		_ekf->get_flow_innov_var(&innovations.flow_innov_var[0]);
 		_ekf->get_hagl_innov_var(&innovations.hagl_innov_var);
+
 		if (_estimator_innovations_pub == nullptr) {
 			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
 
@@ -658,6 +661,7 @@ void Ekf2::task_main()
 
 		// publish replay message if in replay mode
 		bool publish_replay_message = (bool)_param_record_replay_msg->get();
+
 		if (publish_replay_message) {
 			struct ekf2_replay_s replay = {};
 			replay.time_ref = now;
@@ -666,7 +670,8 @@ void Ekf2::task_main()
 			replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
 			replay.baro_timestamp = sensors.baro_timestamp[0];
 			memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
-			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s));
+			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
+			       sizeof(replay.accelerometer_integral_m_s));
 			memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
 			replay.baro_alt_meter = sensors.baro_alt_meter[0];
 
@@ -687,6 +692,7 @@ void Ekf2::task_main()
 				replay.vel_e_m_s = gps.vel_e_m_s;
 				replay.vel_d_m_s = gps.vel_d_m_s;
 				replay.vel_ned_valid = gps.vel_ned_valid;
+
 			} else {
 				// this will tell the logging app not to bother logging any gps replay data
 				replay.time_usec = 0;
@@ -700,6 +706,7 @@ void Ekf2::task_main()
 				replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
 				replay.flow_time_integral = optical_flow.integration_timespan;
 				replay.flow_quality = optical_flow.quality;
+
 			} else {
 				replay.flow_timestamp = 0;
 			}
@@ -707,6 +714,7 @@ void Ekf2::task_main()
 			if (range_finder_updated) {
 				replay.rng_timestamp = range_finder.timestamp;
 				replay.range_to_ground = range_finder.current_distance;
+
 			} else {
 				replay.rng_timestamp = 0;
 			}