added 5837 support in 5611 code.

timzarhansen · Oct 15, 2023 · 33ed847 · 33ed847
1 parent cc5f52a
commit 33ed847
Show file tree

Hide file tree

Showing 9 changed files with 130 additions and 41 deletions.
diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board
@@ -16,6 +16,7 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_COMMON_HYGROMETERS=y
+CONFIG_DRIVERS_BAROMETER_MS5837=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y

diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors
@@ -22,8 +22,9 @@ bmi055 -A -R 2 -s start
 bmi055 -G -R 2 -s start
 
 # Baro on internal SPI
-ms5611 -s start
-
+#ms5611 -s start
+# Baro on External BlueROBOTICS
+ms5837 -X start
 # internal compass
 ist8310 -I -R 10 start
 

diff --git a/src/drivers/barometer/ms5611/MS5611.hpp b/src/drivers/barometer/ms5611/MS5611.hpp
@@ -46,7 +46,8 @@
 enum MS56XX_DEVICE_TYPES {
 	MS5611_DEVICE	= 5611,
 	MS5607_DEVICE	= 5607,
-};
+    MS5837_DEVICE   = 5837,
+    };
 
 /* helper macro for handling report buffer indices */
 #define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)

diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp
@@ -131,6 +131,10 @@ MS5611::init()
 		case MS5607_DEVICE:
 			_interface->set_device_type(DRV_BARO_DEVTYPE_MS5607);
 			break;
+
+        case MS5837_DEVICE:
+            _interface->set_device_type(DRV_BARO_DEVTYPE_MS5837);
+            break;
 		}
 
 		ret = OK;
@@ -302,39 +306,94 @@ MS5611::collect()
 			}
 
 		} else if (_device_type == MS5607_DEVICE) {
-			/* Perform MS5607 Caculation */
-			_OFF  = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6);
-			_SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7);
+            /* Perform MS5607 Caculation */
+            _OFF = ((int64_t) _prom.c2_pressure_offset << 17) + (((int64_t) _prom.c4_temp_coeff_pres_offset * dT) >> 6);
+            _SENS = ((int64_t) _prom.c1_pressure_sens << 16) + (((int64_t) _prom.c3_temp_coeff_pres_sens * dT) >> 7);
 
-			/* MS5607 temperature compensation */
-			if (TEMP < 2000) {
+            /* MS5607 temperature compensation */
+            if (TEMP < 2000) {
 
-				int32_t T2 = POW2(dT) >> 31;
+                int32_t T2 = POW2(dT) >> 31;
 
-				int64_t f = POW2((int64_t)TEMP - 2000);
-				int64_t OFF2 = 61 * f >> 4;
-				int64_t SENS2 = 2 * f;
+                int64_t f = POW2((int64_t) TEMP - 2000);
+                int64_t OFF2 = 61 * f >> 4;
+                int64_t SENS2 = 2 * f;
 
-				if (TEMP < -1500) {
-					int64_t f2 = POW2(TEMP + 1500);
-					OFF2 += 15 * f2;
-					SENS2 += 8 * f2;
-				}
+                if (TEMP < -1500) {
+                    int64_t f2 = POW2(TEMP + 1500);
+                    OFF2 += 15 * f2;
+                    SENS2 += 8 * f2;
+                }
 
-				TEMP -= T2;
-				_OFF  -= OFF2;
-				_SENS -= SENS2;
-			}
-		}
+                TEMP -= T2;
+                _OFF -= OFF2;
+                _SENS -= SENS2;
+            }
+        } else if (_device_type == MS5837_DEVICE) {
+        /* Perform MS5837 Caculation */
+        /* Similar to MS5611 but with second order temperature compensation for high temperatures. Have fun in warm water! */
+        _OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
+        _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+
+        /* MS5837 temperature compensation */
+        if (TEMP < 2000) {
+
+            int64_t T2 = 3 * POW2((int64_t)dT) >> 33;
+
+            int64_t f = POW2((int64_t)TEMP - 2000);
+            int64_t OFF2 = 3 * f >> 1;
+            int64_t SENS2 = 5 * f >> 3;
+
+            if (TEMP < -1500) {
+
+                int64_t f2 = POW2(TEMP + 1500);
+                OFF2 += 7 * f2;
+                SENS2 += 4 * f2;
+            }
+
+            TEMP -= T2;
+            _OFF  -= OFF2;
+            _SENS -= SENS2;
+
+        } else if (TEMP >= 2000) {
+            /* Casting because shifting 37 bits makes little sense on int32 */
+            int64_t T2 = 2 * POW2((int64_t)dT) >> 37;
+
+            int64_t f = POW2((int64_t)TEMP - 2000) >> 4;
+            int64_t OFF2 = 3 * f >> 1;;
+
+            TEMP -= T2;
+            _OFF  -= OFF2;
+        }
+
+    }
 
 		_last_temperature = TEMP / 100.0f;
 
 	} else {
 		/* pressure calculation, result in Pa */
-		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+//		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
 
-		_last_pressure = P;
+//		_last_pressure = P;
+        float pressure;
+        int32_t P;
 
+        if (_device_type == MS5837_DEVICE) {
+            /* MS5837, 0.1 mbar resolution */
+            /* pressure calculation, result in Pa */
+            P = (((raw * _SENS) >> 21) - _OFF) >> 13;
+
+            pressure = P / 10.0f;		/* convert to millibar */
+
+        } else {
+            /* MS5611 or MS5607, 0.01 mbar resolution */
+            /* pressure calculation, result in Pa */
+            P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+
+            pressure = P / 100.0f;		/* convert to millibar */
+        }
+        P = pressure;
+        _last_pressure = P;
 		// publish
 		if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) {
 			sensor_baro_s sensor_baro{};
@@ -362,8 +421,25 @@ void MS5611::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
+    char device_name[10] {};
+
+    switch (_device_type) {
+        case MS5611_DEVICE:
+            strncpy(device_name, "ms5611", sizeof(device_name));
+            break;
+
+        case MS5607_DEVICE:
+            strncpy(device_name, "ms5607", sizeof(device_name));
+            break;
+
+        case MS5837_DEVICE:
+            strncpy(device_name, "ms5837", sizeof(device_name));
+            break;
+    }
+
+    printf("device:         %s\n", device_name);
 
-	printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
+//	printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
 }
 
 namespace ms5611

diff --git a/src/drivers/barometer/ms5611/ms5611_main.cpp b/src/drivers/barometer/ms5611/ms5611_main.cpp
@@ -92,7 +92,7 @@ void MS5611::print_usage()
 #else
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
 #endif
-	PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611", "Device type", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611|5837", "Device type", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
@@ -120,7 +120,9 @@ extern "C" int ms5611_main(int argc, char *argv[])
 
 				} else if (val == 5607) {
 					dev_type_driver = DRV_BARO_DEVTYPE_MS5607;
-				}
+				}else if (val == 5837) {
+                    dev_type_driver = DRV_BARO_DEVTYPE_MS5837;
+                }
 			}
 			break;
 		}

diff --git a/src/drivers/barometer/ms5837/MS5837.cpp b/src/drivers/barometer/ms5837/MS5837.cpp
@@ -340,7 +340,7 @@ int MS5837::_collect()
 		sensor_baro.timestamp_sample = timestamp_sample;
 		sensor_baro.device_id = get_device_id();
 		sensor_baro.pressure = P;
-		sensor_baro.temperature = T;
+		sensor_baro.temperature = _last_temperature;
 		sensor_baro.error_count = perf_event_count(_comms_errors);
 		sensor_baro.timestamp = hrt_absolute_time();
 		_sensor_baro_pub.publish(sensor_baro);

diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
@@ -298,8 +298,8 @@ void VehicleAirData::Run()
 float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const
 {
 	// calculate altitude using the hypsometric equation
-	static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
-	static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
+//	static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
+//	static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
 
 	// current pressure at MSL in kPa (QNH in hPa)
 	const float p1 = _param_sens_baro_qnh.get() * 0.1f;
@@ -316,8 +316,8 @@ float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) c
 	 * h = -------------------------------  + h1
 	 *                   a
 	 */
-	float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
-
+//	float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
+    float altitude = -((p-p1)*1000.0f)/(CONSTANTS_ONE_G*1000.0f);//calculation for water
 	return altitude;
 }
 

diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp
@@ -73,9 +73,6 @@ bool UUVAttitudeControl::init()
 		PX4_ERR("callback registration failed");
 		return false;
 	}
-    this->errorVectorIntegrated(0) = 0;
-    this->errorVectorIntegrated(1) = 0;
-    this->errorVectorIntegrated(2) = 0;
 	return true;
 }
 
@@ -127,18 +124,18 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 		_vehicle_thrust_setpoint.xyz[0] = 0.0f;
 	}
     if (PX4_ISFINITE(thrust_y)) {
-        thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
-        _vehicle_thrust_setpoint.xyz[0] = thrust_x;
+        thrust_y = math::constrain(thrust_y, -1.0f, 1.0f);
+        _vehicle_thrust_setpoint.xyz[1] = thrust_y;
 
     } else {
-        _vehicle_thrust_setpoint.xyz[0] = 0.0f;
+        _vehicle_thrust_setpoint.xyz[1] = 0.0f;
     }
     if (PX4_ISFINITE(thrust_z)) {
-        thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
-        _vehicle_thrust_setpoint.xyz[0] = thrust_x;
+        thrust_z = math::constrain(thrust_z, -1.0f, 1.0f);
+        _vehicle_thrust_setpoint.xyz[2] = thrust_z;
 
     } else {
-        _vehicle_thrust_setpoint.xyz[0] = 0.0f;
+        _vehicle_thrust_setpoint.xyz[2] = 0.0f;
     }
 }
 
@@ -207,6 +204,11 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 	pitch_u = torques(1);
 	yaw_u = torques(2);
 
+
+    thrust_x = attitude_setpoint.thrust_body[0];
+    thrust_y = attitude_setpoint.thrust_body[1];
+    thrust_z = attitude_setpoint.thrust_body[2];
+
     // new i think thats better
     Vector3f thrustVector(thrust_x,thrust_y,thrust_z);
     Dcmf rot_Of = Eulerf(euler_angles.phi(), euler_angles.theta(), 0);

diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml
@@ -47,6 +47,12 @@ publications:
   - topic: /fmu/out/vehicle_trajectory_waypoint_desired
     type: px4_msgs::msg::VehicleTrajectoryWaypoint
 
+  - topic: /fmu/out/sensor_baro
+    type: px4_msgs::msg::SensorBaro
+
+  - topic: /fmu/out/vehicle_air_data
+    type: px4_msgs::msg::VehicleAirData
+
 subscriptions:
 
   - topic: /fmu/in/offboard_control_mode