Update to support GPS disabled, VIO only based missions that are plan…

…ned normally in global space/NED

src/modules/commander/Commander.cpp

@@ -702,6 +702,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
+					PX4_ERR("vehicle_command_s::VEHICLE_CMD_DO_REPOSITION");
 					printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
@@ -723,6 +724,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
+				PX4_ERR("vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE");
+
 				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
@@ -731,6 +734,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
+
+			PX4_ERR("vehicle_command_s::VEHICLE_CMD_DO_SET_MODE");
+
 			uint8_t base_mode = (uint8_t)cmd.param1;
 			uint8_t custom_main_mode = (uint8_t)cmd.param2;
 			uint8_t custom_sub_mode = (uint8_t)cmd.param3;
@@ -827,6 +833,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else {
 					if (cmd.from_external && cmd.source_component == 190) { // MAV_COMP_ID_MISSIONPLANNER
+						PX4_ERR("MAV_COMP_ID_MISSIONPLANNER");
+
 						printRejectMode(desired_nav_state);
 					}
 
@@ -932,6 +940,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
+
+			PX4_ERR("vehicle_command_s::VEHICLE_CMD_DO_SET_HOME");
+
 			if (_param_com_home_en.get()) {
 				bool use_current = cmd.param1 > 0.5f;
 
@@ -953,11 +964,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 					if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
 
+						PX4_ERR("_home_position.setManually: %f %f %f %f", (double) lat, (double)lon, (double)alt, (double)yaw);
+
 						if (_home_position.setManually(lat, lon, alt, yaw)) {
 
+							PX4_ERR("_home_position.setManually SUCCESS");
 							cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 						} else {
+							PX4_ERR("_home_position.setManually FAIL");
 							cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 						}
 
@@ -981,6 +996,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
+				PX4_ERR("vehicle_status_s::NAVIGATION_STATE_AUTO_RTL");
+
 				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
@@ -993,6 +1010,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
+				PX4_ERR("vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF");
+
 				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
@@ -1006,6 +1025,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else {
+			PX4_ERR("vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF");
+
 			printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 		}
@@ -1034,6 +1055,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
+				PX4_ERR("vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND");
+
 				printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND);
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
@@ -1057,6 +1080,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 					} else {
+						PX4_ERR("vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION");
+
 						printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
 						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 					}
@@ -1554,6 +1579,8 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	case action_request_s::ACTION_SWITCH_MODE:
 
 		if (!_user_mode_intention.change(action_request.mode, true)) {
+			PX4_ERR("action_request_s::ACTION_SWITCH_MODE");
+
 			printRejectMode(action_request.mode);
 		}
 

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -197,6 +197,7 @@ bool Report::finalize()
 
 bool Report::report(bool is_armed, bool force)
 {
+
 	const hrt_abstime now = hrt_absolute_time();
 	const bool has_difference = _had_unreported_difference || _results_changed;
 

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -206,11 +206,31 @@ class Report
 		       (uint32_t)(_results[_current_result].arming_checks.can_arm & getModeGroup(nav_state)) != 0;
 	}
 
+
 	/**
 	 * Whether a navigation mode can be run (while already armed)
 	 */
 	bool canRun(uint8_t nav_state) const
 	{
+
+		PX4_ERR("canRun  %d %d %d %d",
+				_current_result,
+				_results[_current_result].arming_checks.valid,
+				(uint32_t)_results[_current_result].arming_checks.can_run,
+				(uint32_t)getModeGroup(nav_state));
+
+
+		PX4_ERR("canRun DEFs  %d %d %d %d %d %d %d  -- %d %d ",
+				(uint32_t)NavModes::None,
+				(uint32_t)NavModes::Manual,
+				(uint32_t)NavModes::Stabilized,
+				(uint32_t)NavModes::PositionControl,
+				(uint32_t)NavModes::Mission,
+				(uint32_t)NavModes::Takeoff,
+				(uint32_t)NavModes::All,
+				(uint32_t) nav_state,
+				(uint32_t)getModeGroup(nav_state));
+
 		return _results[_current_result].arming_checks.valid &&
 		       (uint32_t)(_results[_current_result].arming_checks.can_run & getModeGroup(nav_state)) != 0;
 	}

src/modules/commander/HealthAndArmingChecks/checks/missionCheck.cpp

@@ -54,7 +54,6 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter)
 				mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t");
 			}
 		}
-
 		// This is a mode requirement, no need to report
 		reporter.failsafeFlags().auto_mission_missing = mission_result.instance_count <= 0;
 	}

src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp

@@ -35,6 +35,7 @@
 
 void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 {
+
 	if (!context.isArmed()) {
 		checkArmingRequirement(context, reporter);
 	}
@@ -109,6 +110,12 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	if (reporter.failsafeFlags().auto_mission_missing && mission_required_modes != NavModes::None) {
+
+//		PX4_ERR("No valid mission! %d %d %d",
+//				(int) reporter.failsafeFlags().auto_mission_missing,
+//				(int)mission_required_modes,
+//				(int)NavModes::None);
+
 		/* EVENT
 		 * @description
 		 * Upload a mission first.
@@ -121,6 +128,8 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 					    events::ID("check_modes_mission"),
 					    events::Log::Info, "No valid mission available");
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
+
+
 	}
 
 	if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) {

src/modules/commander/UserModeIntention.cpp

@@ -48,13 +48,18 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 	bool always_allow = force || !isArmed();
 	bool allow_change = true;
 
+	PX4_ERR("Allow change 0 %d %d %d", allow_change, force, !isArmed());
+
+
 	if (!always_allow) {
 		allow_change = _health_and_arming_checks.canRun(user_intended_nav_state);
+		PX4_ERR("Allow change A %d", allow_change);
 
 		// Check fallback
 		if (!allow_change && allow_fallback && _param_com_posctl_navl.get() == 0) {
 			if (user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
 				allow_change = _health_and_arming_checks.canRun(vehicle_status_s::NAVIGATION_STATE_ALTCTL);
+				PX4_ERR("Allow change B %d", allow_change);
 				// We still use the original user intended mode. The failsafe state machine will then set the
 				// fallback and once can_run becomes true, the actual user intended mode will be selected.
 			}
@@ -68,6 +73,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
 			_nav_state_after_disarming = user_intended_nav_state;
 		}
+		else
+		{
+			PX4_ERR("_health_and_arming_checks.modePreventsArming was true");
+		}
 	}
 
 	return allow_change;

src/modules/ekf2/EKF/ekf.h

@@ -506,6 +506,28 @@ class Ekf final : public EstimatorInterface
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
+	// offset for ev data  aif ev data is only used for position estimation in global NED
+	// basically the yaw value for _R_to_Earth
+	float ev_yaw_offset_2_G_NED = 0.0f;
+	// handle non-gps control in the global coordinate system, NED
+	void forceResetQuatStateYaw(float yaw, float yaw_variance)
+	{
+		resetQuatStateYaw( yaw,  yaw_variance);
+		ev_yaw_offset_2_G_NED = yaw;
+
+		// Reset global yaw by user input
+		// TODO This is experimental!
+		/////////////////////////////////////
+		_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
+		_state.quat_nominal = _R_to_earth;
+		// reset the output predictor state history to match the EKF initial values
+		_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
+
+		/////////////////////////////////////
+
+	}
+
+
 private:
 
 	// set the internal states and status to their default value

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -404,6 +404,7 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	latitude = _pos_ref.getProjectionReferenceLat();
 	longitude = _pos_ref.getProjectionReferenceLon();
 	origin_alt  = getEkfGlobalOriginAltitude();
+
 	return _NED_origin_initialised;
 }
 
@@ -454,6 +455,10 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
 		}
 
+		// sim GPS as needed
+		PX4_WARN("Forcing user based origin in NED");
+		_NED_origin_initialised = true;
+
 		return true;
 	}
 

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 	resetEstimatorAidStatus(aid_src);
 	aid_src.timestamp_sample = ev_sample.time_us;
-	aid_src.observation = getEulerYaw(ev_sample.quat);
+	aid_src.observation = getEulerYaw(ev_sample.quat) + ev_yaw_offset_2_G_NED;
 	aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
 	aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
 

src/modules/ekf2/EKF2.cpp

@@ -468,21 +468,32 @@ void EKF2::Run()
 
 		if (_vehicle_command_sub.update(&vehicle_command)) {
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
+
+				PX4_ERR("Attempting to reset EKF global origin!");
 				double latitude = vehicle_command.param5;
 				double longitude = vehicle_command.param6;
 				float altitude = vehicle_command.param7;
 
 				if (_ekf.setEkfGlobalOrigin(latitude, longitude, altitude)) {
 					// Validate the ekf origin status.
 					uint64_t origin_time {};
-					_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
-					PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
-						 _instance, latitude, longitude, static_cast<double>(altitude));
+					double x,y;
+					float z;
+					bool rtn = _ekf.getEkfGlobalOrigin(origin_time, x, y, z);
+					PX4_ERR("%d %d - SET UP! NED origin (LLA): %3.10f, %3.10f, %4.3f\n\n",
+						 rtn, _instance, x, y, static_cast<double>(z));
+					print_status();
 
 				} else {
 					PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
 						_instance, latitude, longitude, static_cast<double>(altitude));
 				}
+
+
+				// TODO fix EV yaw value as it becomes used as global not local.
+				//PX4_ERR("Attempting to reset EKF global Yaw!");
+				_ekf.forceResetQuatStateYaw(vehicle_command.param4, 0.0);
+
 			}
 		}
 	}