commander: «home position set» condition fixed, failsafe fixes, navigator: state indication bugfix, control_mode fixes

src/modules/commander/commander.cpp

@@ -565,7 +565,6 @@ int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
 	commander_initialized = false;
-	bool home_position_set = false;
 
 	bool battery_tune_played = false;
 	bool arm_tune_played = false;
@@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[])
 	char *failsafe_states_str[FAILSAFE_STATE_MAX];
 	failsafe_states_str[0] = "NORMAL";
 	failsafe_states_str[1] = "RTL";
-	failsafe_states_str[2] = "TERMINATION";
+	failsafe_states_str[2] = "LAND";
+	failsafe_states_str[3] = "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
+	status.failsafe_state = FAILSAFE_STATE_NORMAL;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[])
 			 * position to the current position.
 			 */
 
-			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
 			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
 			    && global_position.valid) {
-				/* copy position data to uORB home message, store it locally as well */
-
 
+				/* copy position data to uORB home message, store it locally as well */
 				home.lat = global_position.lat;
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
@@ -1050,15 +1050,13 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* mark home position as set */
-				home_position_set = true;
+				status.condition_home_position_valid = true;
 				tune_positive();
 			}
 		}
 
-		/* ignore RC signals if in offboard control mode */
-		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
 		/* start RC input check */
-			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (sp_man.timestamp != 0 && 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;
@@ -1191,7 +1189,6 @@ int commander_thread_main(int argc, char *argv[])
 			} else if (armed.armed) {
 				/* failsafe for manual modes */
 				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-
 				if (res == TRANSITION_CHANGED) {
 					mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
 
@@ -1212,7 +1209,6 @@ int commander_thread_main(int argc, char *argv[])
 				}
 			}
 		}
-		}
 
 		// TODO remove this hack
 		/* flight termination in manual mode if assisted switch is on easy position */

src/modules/navigator/navigator_main.cpp

@@ -411,11 +411,13 @@ Navigator::Navigator() :
 	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 	memset(&_mission_item, 0, sizeof(struct mission_item_s));
 
+	memset(&nav_states_str, 0, sizeof(nav_states_str));
 	nav_states_str[0] = "NONE";
 	nav_states_str[1] = "READY";
 	nav_states_str[2] = "LOITER";
 	nav_states_str[3] = "MISSION";
 	nav_states_str[4] = "RTL";
+	nav_states_str[5] = "LAND";
 
 	/* Initialize state machine */
 	myState = NAV_STATE_NONE;
@@ -750,6 +752,7 @@ Navigator::task_main()
 				} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
 					/* RTL on failsafe */
 					if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
+
 						dispatch(EVENT_RTL_REQUESTED);
 					}
 
@@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
 		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
 	},
@@ -1220,7 +1223,7 @@ Navigator::start_land()
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
 	_pos_sp_triplet.current.yaw = NAN;
 
-	mavlink_log_info(_mavlink_fd, "[navigator] LAND started");
+	mavlink_log_info(_mavlink_fd, "[navigator] land started");
 }
 
 void
@@ -1616,14 +1619,7 @@ Navigator::publish_control_mode()
 		break;
 
 	case FAILSAFE_STATE_LAND:
-		/* land with or without position control */
-		_control_mode.flag_control_manual_enabled = false;
-		_control_mode.flag_control_rates_enabled = true;
-		_control_mode.flag_control_attitude_enabled = true;
-		_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
-		_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
-		_control_mode.flag_control_altitude_enabled = true;
-		_control_mode.flag_control_climb_rate_enabled = true;
+		navigator_enabled = true;
 		break;
 
 	case FAILSAFE_STATE_TERMINATION:
@@ -1647,7 +1643,8 @@ Navigator::publish_control_mode()
 	if (navigator_enabled) {
 		_control_mode.flag_control_manual_enabled = false;
 
-		if (myState == NAV_STATE_READY) {
+		switch (myState) {
+		case NAV_STATE_READY:
 			/* disable all controllers, armed but idle */
 			_control_mode.flag_control_rates_enabled = false;
 			_control_mode.flag_control_attitude_enabled = false;
@@ -1655,14 +1652,27 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_velocity_enabled = false;
 			_control_mode.flag_control_altitude_enabled = false;
 			_control_mode.flag_control_climb_rate_enabled = false;
+			break;
 
-		} else {
+		case NAV_STATE_LAND:
+			/* land with or without position control */
+			_control_mode.flag_control_manual_enabled = false;
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
+			_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
+			_control_mode.flag_control_altitude_enabled = true;
+			_control_mode.flag_control_climb_rate_enabled = true;
+			break;
+
+		default:
 			_control_mode.flag_control_rates_enabled = true;
 			_control_mode.flag_control_attitude_enabled = true;
 			_control_mode.flag_control_position_enabled = true;
 			_control_mode.flag_control_velocity_enabled = true;
 			_control_mode.flag_control_altitude_enabled = true;
 			_control_mode.flag_control_climb_rate_enabled = true;
+			break;
 		}
 	}