commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup

3 files changed, 74 insertions, 85 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 872939d6d..d40f6675b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,12 +117,9 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
+#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define RC_TIMEOUT 100000
+#define DIFFPRESS_TIMEOUT 2000000
 
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	2000000
@@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
+void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
+
 void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position);
 
 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
@@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_NAV_TAKEOFF: {
-		transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-		if (nav_res == TRANSITION_CHANGED) {
-			mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
-		}
+			transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 
-		if (nav_res != TRANSITION_DENIED) {
-			result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (nav_res == TRANSITION_CHANGED) {
+				mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+			}
 
-		} else {
-			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			if (nav_res != TRANSITION_DENIED) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
 		}
-		break;
-	}
 
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 		if (is_safe(status, safety, armed)) {
@@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[])
 	start_time = hrt_absolute_time();
 
 	while (!thread_should_exit) {
-		hrt_abstime t = hrt_absolute_time();
-
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			last_diff_pres_time = diff_pres.timestamp;
 		}
 
-		/* Check for valid airspeed/differential pressure measurements */
-		if (t - last_diff_pres_time < 2000000 && t > 2000000) {
-			status.condition_airspeed_valid = true;
-
-		} else {
-			status.condition_airspeed_valid = false;
-		}
+		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
 		orb_check(cmd_sub, &updated);
 
@@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
-		/* set the condition to valid if there has recently been a global position estimate */
-		if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) {
-			if (!status.condition_global_position_valid) {
-				status.condition_global_position_valid = true;
-				status_changed = true;
-			}
-
-		} else {
-			if (status.condition_global_position_valid) {
-				status.condition_global_position_valid = false;
-				status_changed = true;
-			}
-		}
+		/* update condition_global_position_valid */
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
 		}
 
-		/* set the condition to valid if there has recently been a local position estimate */
-		if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) {
-			if (!status.condition_local_position_valid) {
-				status.condition_local_position_valid = true;
-				status_changed = true;
-			}
-
-		} else {
-			if (status.condition_local_position_valid) {
-				status.condition_local_position_valid = false;
-				status_changed = true;
-			}
-		}
+		/* update condition_local_position_valid */
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed);
 
 		/* update battery status */
 		orb_check(battery_sub, &updated);
@@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[])
 			warnx("bat v: %2.2f", battery.voltage_v);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
-			if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) {
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
 				status.battery_voltage = battery.voltage_v;
 				status.condition_battery_voltage_valid = true;
 				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[])
 			 * position to the current position.
 			 */
 
-			if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D &&
+			if (!home_position_set && gps_position.fix_type >= 3 &&
 			    (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
-			    (t - gps_position.timestamp_position < 2000000)
-			    && !armed.armed) {
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
 				/* copy position data to uORB home message, store it locally as well */
 				// TODO use global position estimate
 				home.lat = gps_position.lat;
@@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* ignore RC signals if in offboard control mode */
 		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
 			/* start RC input check */
-			if ((t - sp_man.timestamp) < 100000) {
+			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 				/* handle the case where RC signal was regained */
 				if (!status.rc_signal_found_once) {
 					status.rc_signal_found_once = true;
@@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[])
 			} else {
 
 				/* print error message for first RC glitch and then every 5s */
-				if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) {
+				if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) {
 					// TODO remove debug code
 					if (!status.rc_signal_cutting_off) {
 						warnx("Reason: not rc_signal_cutting_off\n");
@@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[])
 					status.rc_signal_cutting_off = true;
 					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
 
-					last_print_control_time = t;
+					last_print_control_time = hrt_absolute_time();
 				}
 
 				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				status.rc_signal_lost_interval = t - sp_man.timestamp;
+				status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
 
 				/* if the RC signal is gone for a full second, consider it lost */
 				if (status.rc_signal_lost_interval > 1000000) {
@@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[])
 		// TODO check this
 		/* state machine update for offboard control */
 		if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((t - sp_offboard.timestamp) < 5000000) {	// TODO 5s is too long ?
+			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {	// TODO 5s is too long ?
 
 				// /* decide about attitude control flag, enable in att/pos/vel */
 				// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
@@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[])
 			} else {
 
 				/* print error message for first offboard signal glitch and then every 5s */
-				if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) {
+				if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) {
 					status.offboard_control_signal_weak = true;
 					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
-					last_print_control_time = t;
+					last_print_control_time = hrt_absolute_time();
 				}
 
 				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp;
+				status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
 
 				/* if the signal is gone for 0.1 seconds, consider it lost */
 				if (status.offboard_control_signal_lost_interval > 100000) {
 					status.offboard_control_signal_lost = true;
-					status.failsave_lowlevel_start_time = t;
+					status.failsave_lowlevel_start_time = hrt_absolute_time();
 					tune_positive();
 
 					/* kill motors after timeout */
-					if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
+					if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) {
 						status.failsave_lowlevel = true;
 						status_changed = true;
 					}
@@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
+		hrt_abstime t1 = hrt_absolute_time();
+
 		/* publish arming state */
 		if (arming_state_changed) {
-			armed.timestamp = t;
+			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
@@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[])
 		if (navigation_state_changed) {
 			/* publish new navigation state */
 			control_mode.counter++;
-			control_mode.timestamp = t;
+			control_mode.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 		}
 
 		/* publish vehicle status at least with 1 Hz */
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.counter++;
-			status.timestamp = t;
+			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 			status_changed = false;
 		}
@@ -1341,6 +1312,18 @@ int commander_thread_main(int argc, char *argv[])
 }
 
 void
+check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
+{
+	hrt_abstime t = hrt_absolute_time();
+	bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
+
+	if (*valid_out != valid_new) {
+		*valid_out = valid_new;
+		*changed = true;
+	}
+}
+
+void
 toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position)
 {
 	if (leds_counter % 2 == 0) {
@@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp
 			/* position estimated, solid */
 			led_on(LED_BLUE);
 
-		} else if (leds_counter == 0) {
+		} else if (leds_counter == 6) {
 			/* waiting for position estimate, short blink at 1.25Hz */
 			led_on(LED_BLUE);
 
@@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 				/* don't act while taking off */
 				res = TRANSITION_NOT_CHANGED;
+
 			} else {
 				if (current_status->condition_landed) {
 					/* if landed: transitions only to AUTO_READY are allowed */
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 1aa24a4fc..5cad667f6 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[])
 					}
 
 					/* apply controller */
-					float gyro[3];
-					gyro[0] = att.rollspeed;
-					gyro[1] = att.pitchspeed;
-					gyro[2] = att.yawspeed;
-
-					multirotor_control_rates(&rates_sp, gyro, &actuators);
-					orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+					float rates[3];
+					rates[0] = att.rollspeed;
+					rates[1] = att.pitchspeed;
+					rates[2] = att.yawspeed;
+					multirotor_control_rates(&rates_sp, rates, &actuators);
+				} else {
+					/* rates controller disabled, set actuators to zero for safety */
+					actuators.control[0] = 0.0f;
+					actuators.control[1] = 0.0f;
+					actuators.control[2] = 0.0f;
+					actuators.control[3] = 0.0f;
 				}
+				actuators.timestamp = hrt_absolute_time();
+				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
 				/* update state */
 				flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 0530c2dea..0e7e0ef5d 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
+static hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
 
 		/* dont't try to estimate position when no any position source available */
-		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
+		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout;
 
 		if (can_estimate_pos) {
 			/* inertial filter prediction for position */
@@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
 			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
 
-			if (params.use_gps && gps.fix_type >= 3) {
-				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
-				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+			inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+			inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
 
-				if (gps.vel_ned_valid) {
-					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
-					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
-				}
+			if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) {
+				inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+				inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
 			}
 		}