Merge branch 'release_v1.0.0'

makefiles/nuttx/config_aerocore_default.mk

@@ -48,11 +48,12 @@ MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 
 #
-# Estimation modules (EKF/ SO3 / other filters)
+# Estimation modules (EKF / other filters)
 #
-MODULES		+= modules/attitude_estimator_ekf
+# Too high RAM usage due to static allocations
-MODULES		+= modules/attitude_estimator_so3
+#MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/ekf_att_pos_estimator
+MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/position_estimator_inav
 
 #

makefiles/nuttx/config_px4fmu-v2_default.mk

@@ -77,7 +77,8 @@ MODULES 	+= modules/land_detector
 #
 # Estimation modules (EKF/ SO3 / other filters)
 #
-MODULES		+= modules/attitude_estimator_ekf
+# Too high RAM usage due to static allocations
+#MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav

src/modules/commander/commander.cpp

@@ -1485,7 +1485,22 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+			vehicle_global_position_s gpos;
+			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
+
+			/* copy to global struct if valid, with hysteresis */
+
+			// XXX consolidate this with local position handling and timeouts after release
+			// but we want a low-risk change now.
+			if (status.condition_global_position_valid) {
+				if (gpos.eph < eph_threshold * 2.5f) {
+					orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+				}
+			} else {
+				if (gpos.eph < eph_threshold) {
+					orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+				}
+			}
 		}
 
 		/* update local position estimate */
@@ -1498,17 +1513,16 @@ int commander_thread_main(int argc, char *argv[])
 
 		//update condition_global_position_valid
 		//Global positions are only published by the estimators if they are valid
-		if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
+		if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
 			//We have had no good fix for POSITION_TIMEOUT amount of time
-			if(status.condition_global_position_valid) {
+			if (status.condition_global_position_valid) {
 				set_tune_override(TONE_GPS_WARNING_TUNE);
 				status_changed = true;
 				status.condition_global_position_valid = false;
 			}
-		}
+		} else if (global_position.timestamp != 0) {
-		else if(global_position.timestamp != 0) {
+			// Got good global position estimate
-			//Got good global position estimate
+			if (!status.condition_global_position_valid) {
-			if(!status.condition_global_position_valid) {
 				status_changed = true;
 				status.condition_global_position_valid = true;
 			}
@@ -2642,7 +2656,8 @@ set_control_mode()
 
 		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
 
-		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
+		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
+			!offboard_control_mode.ignore_position;
 
 		break;
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -78,6 +78,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f;				///< Seconds before we sig
 static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10;	///< Ignore the first few mag failures (which amounts to a few milliseconds)
 static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5;	///< Ignore the first few gyro failures (which amounts to a few milliseconds)
 static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5;	///< Ignore the first few accel failures (which amounts to a few milliseconds)
+static constexpr float EPH_LARGE_VALUE = 1000.0f;
+static constexpr float EPV_LARGE_VALUE = 1000.0f;
 
 /**
  * estimator app start / stop handling function
@@ -936,8 +938,16 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
 	_global_pos.yaw = _local_pos.yaw;
-	_global_pos.eph = _gps.eph;
+
-	_global_pos.epv = _gps.epv;
+	const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
+
+	if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
+		_global_pos.eph = EPH_LARGE_VALUE;
+		_global_pos.epv = EPV_LARGE_VALUE;
+	} else {
+		_global_pos.eph = _gps.eph;
+		_global_pos.epv = _gps.epv;
+	}
 
 	if (!PX4_ISFINITE(_global_pos.lat) ||
 		!PX4_ISFINITE(_global_pos.lon) ||
@@ -1436,7 +1446,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
 	// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
-	const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+	const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
 
 	if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
 

src/modules/px4iofirmware/mixer.cpp

@@ -287,15 +287,18 @@ mixer_tick(void)
 
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
 		/* set the disarmed servo outputs. */
-		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
+		}
 
 		/* set S.BUS1 or S.BUS2 outputs */
-		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+			sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+		}
 
-		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+			sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+		}
 	}
 }
 

src/modules/sdlog2/sdlog2_messages.h

@@ -218,7 +218,6 @@ struct log_ARSP_s {
 /* --- FLOW - OPTICAL FLOW --- */
 #define LOG_FLOW_MSG 15
 struct log_FLOW_s {
-	uint64_t timestamp;
 	uint8_t sensor_id;
 	float pixel_flow_x_integral;
 	float pixel_flow_y_integral;
@@ -522,7 +521,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
-	LOG_FORMAT(FLOW, "QBffffffLLHhB",	"IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
+	LOG_FORMAT(FLOW, "BffffffLLHhB",	"ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),

src/platforms/ros/nodes/commander/commander.cpp

@@ -135,7 +135,8 @@ void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboar
 
 	msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
 
-	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity ||
+		!msg_offboard_control_mode.ignore_position;
 }
 
 void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,