5th batch of commit

src/drivers/imu/Kconfig

@@ -8,7 +8,6 @@ menu "IMU"
         select DRIVERS_IMU_ANALOG_DEVICES_ADIS16470
         select DRIVERS_IMU_BOSCH_BMI055
         select DRIVERS_IMU_BOSCH_BMI088
-        select DRIVERS_IMU_MURATA_SCH16T
         select DRIVERS_IMU_NXP_FXAS21002C
         select DRIVERS_IMU_NXP_FXOS8701CQ
         select DRIVERS_IMU_INVENSENSE_ICM20602

src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp

@@ -172,9 +172,7 @@ void ADIS16507::RunImpl()
 			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
 
 			if (DIAG_STAT != 0) {
-				PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT);
-				_state = STATE::RESET;
-				ScheduleDelayed(3_s);
+				PX4_ERR("DIAG_STAT: %#X", DIAG_STAT);
 
 			} else {
 				PX4_DEBUG("self test passed");

src/drivers/ins/vectornav/VectorNav.cpp

@@ -441,11 +441,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			sensor_gps.altitude_msl_m = positionGpsLla.c[2];
 			sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m;
 
-			sensor_gps.vel_m_s = matrix::Vector3f(velocityGpsNed.c).length();
+			sensor_gps.vel_ned_valid = true;
 			sensor_gps.vel_n_m_s = velocityGpsNed.c[0];
 			sensor_gps.vel_e_m_s = velocityGpsNed.c[1];
 			sensor_gps.vel_d_m_s = velocityGpsNed.c[2];
-			sensor_gps.vel_ned_valid = true;
 
 			sensor_gps.hdop = dop.hDOP;
 			sensor_gps.vdop = dop.vDOP;
@@ -832,7 +831,7 @@ Serial bus driver for the VectorNav VN-100, VN-200, VN-300.
 
 Most boards are configured to enable/start the driver on a specified UART using the SENS_VN_CFG parameter.
 
-Setup/usage information: https://docs.px4.io/main/en/sensor/vectornav.html
+Setup/usage information: https://docs.px4.io/master/en/sensor/vectornav.html
 
 ### Examples
 

src/drivers/magnetometer/isentek/ist8310/IST8310.cpp

@@ -85,27 +85,10 @@ void IST8310::print_status()
 
 int IST8310::probe()
 {
-	uint8_t id = RegisterRead(Register::WAI);
-
-	if (id != Device_ID) {
-		DEVICE_DEBUG("unexpected WAI 0x%02x", id);
-
-		// Apparently, the IST8310's WHOAMI register is writeable. Presumably,
-		// this can get corrupted by bus noise. It is only reset if powered off
-		// for 30s or by a reset.
-		RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
-
-		auto start_time = hrt_absolute_time();
-
-		while (hrt_elapsed_time(&start_time) < 50_ms) {
-			px4_usleep(10'000);
-			id = RegisterRead(Register::WAI);
-
-			if (id == Device_ID) {
-				return PX4_OK;
-			}
-		}
+	const uint8_t WAI = RegisterRead(Register::WAI);
 
+	if (WAI != Device_ID) {
+		DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
 		return PX4_ERROR;
 	}
 

src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp

@@ -87,6 +87,14 @@ void PowerMonitorSelectorAuterion::Run()
 			int ret_val = ina226_probe(i);
 
 			if (ret_val == PX4_OK) {
+
+				float current_shunt_value = 0.0f;
+				param_get(param_find("INA226_SHUNT"), &current_shunt_value);
+
+				if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) {
+					param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value));
+				}
+
 				char bus_number[4] = {0};
 				itoa(_sensors[i].bus_number, bus_number, 10);
 				const char *start_argv[] {

src/drivers/smart_battery/batmon/batmon.cpp

@@ -240,7 +240,7 @@ int Batmon::get_batmon_startup_info()
 	_cell_count = math::min((uint8_t)num_cells, (uint8_t)MAX_CELL_COUNT);
 
 	int32_t _num_cells = num_cells;
-	param_set(param_find("BAT1_N_CELLS"), &_num_cells);
+	param_set(param_find("BAT_N_CELLS"), &_num_cells);
 
 	return ret;
 }

src/drivers/telemetry/CMakeLists.txt

@@ -34,4 +34,4 @@
 add_subdirectory(bst)
 add_subdirectory(frsky_telemetry)
 add_subdirectory(hott)
-add_subdirectory(iridiumsbd)
+#add_subdirectory(iridiumsbd)

src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp

@@ -235,7 +235,7 @@ int IridiumSBD::print_status()
 	PX4_INFO("RX session pending:           %d", _rx_session_pending);
 	PX4_INFO("RX read pending:              %d", _rx_read_pending);
 	PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check);
-	PX4_INFO("Last heartbeat:               %" PRId64, _last_at_ok_timestamp);
+	PX4_INFO("Last heartbeat:               %" PRId64, _last_heartbeat);
 	return 0;
 }
 
@@ -333,11 +333,6 @@ void IridiumSBD::standby_loop(void)
 		}
 	}
 
-	if (!is_modem_responsive()) {
-		VERBOSE_INFO("MODEM IS NOT RENSPONSIVE");
-		return;
-	}
-
 	// check for incoming SBDRING, handled inside read_at_command()
 	read_at_command();
 
@@ -482,6 +477,7 @@ void IridiumSBD::sbdsession_loop(void)
 		_ring_pending = false;
 		_tx_session_pending = false;
 		_last_read_time = hrt_absolute_time();
+		_last_heartbeat = _last_read_time;
 		++_successful_sbd_sessions;
 
 		if (mt_queued > 0) {
@@ -502,6 +498,8 @@ void IridiumSBD::sbdsession_loop(void)
 
 	case 1:
 		VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL");
+		_last_heartbeat = hrt_absolute_time();
+
 		// after a successful session reset the tx buffer
 		_tx_buf_write_idx = 0;
 		++_successful_sbd_sessions;
@@ -554,6 +552,11 @@ void IridiumSBD::start_csq(void)
 
 	_last_signal_check = hrt_absolute_time();
 
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
+		return;
+	}
+
 	write_at("AT+CSQ");
 	_new_state = SATCOM_STATE_CSQ;
 }
@@ -568,6 +571,11 @@ void IridiumSBD::start_sbd_session(void)
 		VERBOSE_INFO("STARTING SBD SESSION");
 	}
 
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("SBD SESSION: MODEM NOT READY!");
+		return;
+	}
+
 	if (_ring_pending) {
 		write_at("AT+SBDIXA");
 
@@ -602,8 +610,8 @@ void IridiumSBD::start_test(void)
 		printf("\n");
 	}
 
-	if (!is_modem_responsive()) {
-		PX4_WARN("MODEM NOT RENSPONSIVE!");
+	if (!is_modem_ready()) {
+		PX4_WARN("MODEM NOT READY!");
 		return;
 	}
 
@@ -710,6 +718,11 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)
 
 void IridiumSBD::write_tx_buf()
 {
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("WRITE SBD: MODEM NOT READY!");
+		return;
+	}
+
 	pthread_mutex_lock(&_tx_buf_mutex);
 
 	char command[13];
@@ -766,6 +779,11 @@ void IridiumSBD::write_tx_buf()
 
 void IridiumSBD::read_rx_buf(void)
 {
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("READ SBD: MODEM NOT READY!");
+		return;
+	}
+
 	pthread_mutex_lock(&_rx_buf_mutex);
 
 
@@ -931,12 +949,11 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name)
 	return SATCOM_UART_OK;
 }
 
-bool IridiumSBD::is_modem_responsive(void)
+bool IridiumSBD::is_modem_ready(void)
 {
 	write_at("AT");
 
 	if (read_at_command() == SATCOM_RESULT_OK) {
-		_last_at_ok_timestamp = hrt_absolute_time();
 		return true;
 
 	} else {
@@ -963,9 +980,9 @@ void IridiumSBD::publish_iridium_status()
 {
 	bool need_to_publish = false;
 
-	if (_status.last_at_ok_timestamp != _last_at_ok_timestamp) {
+	if (_status.last_heartbeat != _last_heartbeat) {
 		need_to_publish = true;
-		_status.last_at_ok_timestamp = _last_at_ok_timestamp;
+		_status.last_heartbeat = _last_heartbeat;
 	}
 
 	if (_status.tx_buf_write_index != _tx_buf_write_idx) {

src/drivers/telemetry/iridiumsbd/IridiumSBD.h

@@ -245,7 +245,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
 	/*
 	 * Checks if the modem responds to the "AT" command
 	 */
-	bool is_modem_responsive(void);
+	bool is_modem_ready(void);
 
 	/*
 	 * Get the poll state
@@ -321,7 +321,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
 
 	hrt_abstime _last_write_time = 0;
 	hrt_abstime _last_read_time = 0;
-	hrt_abstime _last_at_ok_timestamp = 0;
+	hrt_abstime _last_heartbeat = 0;
 	hrt_abstime _session_start_time = 0;
 
 	satcom_state _state = SATCOM_STATE_STANDBY;

src/drivers/uavcan/CMakeLists.txt

@@ -168,7 +168,6 @@ px4_add_module(
 		sensors/battery.cpp
 		sensors/airspeed.cpp
 		sensors/flow.cpp
-		sensors/fuel_tank_status.cpp
 		sensors/gnss_relative.cpp
 		sensors/gnss.cpp
 		sensors/mag.cpp

src/drivers/uavcan/Kconfig

@@ -54,10 +54,6 @@ if DRIVERS_UAVCAN
         bool "Subscribe to Flow:                        com::hex::equipment::flow::Measurement"
         default y
 
-    config UAVCAN_SENSOR_FUEL_TANK_STATUS
-        bool "Subscribe to Fuel Tank Status:            uavcan::equipment::ice::FuelTankStatus"
-        default y
-
     config UAVCAN_SENSOR_GNSS
         bool "Subscribe to GPS:                         uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2"
         default y

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -57,9 +57,6 @@
 #if defined(CONFIG_UAVCAN_SENSOR_FLOW)
 #include "flow.hpp"
 #endif
-#if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS)
-#include "fuel_tank_status.hpp"
-#endif
 #if defined(CONFIG_UAVCAN_SENSOR_GNSS)
 #include "gnss.hpp"
 #endif
@@ -140,17 +137,6 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 		list.add(new UavcanFlowBridge(node));
 	}
 
-#endif
-
-	// fuel tank
-#if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS)
-	int32_t uavcan_sub_fuel_tank = 1;
-	param_get(param_find("UAVCAN_SUB_FUEL"), &uavcan_sub_fuel_tank);
-
-	if (uavcan_sub_fuel_tank != 0) {
-		list.add(new UavcanFuelTankStatusBridge(node));
-	}
-
 #endif
 
 	// GPS

src/drivers/uavcan/uavcan_params.c

@@ -97,40 +97,6 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f);
  */
 PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f);
 
-/**
- * UAVCAN fuel tank maximum capacity
- *
- * This parameter defines the maximum fuel capacity of the vehicle's fuel tank.
- *
- * @min 0.0
- * @max 100000.0
- * @unit liters
- * @decimal 1
- * @increment 0.1
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_FLOAT(UAVCAN_ECU_MAXF, 15.0f);
-
-/**
- * UAVCAN fuel tank fuel type
- *
- * This parameter defines the type of fuel used in the vehicle's fuel tank.
- *
- * 0: Unknown
- * 1: Liquid (e.g., gasoline, diesel)
- * 2: Gas (e.g., hydrogen, methane, propane)
- *
- * @min 0
- * @max 2
- * @value 0 Unknown
- * @value 1 Liquid
- * @value 2 Gas
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1);
-
 /**
  * UAVCAN ANTI_COLLISION light operating mode
  *
@@ -326,17 +292,6 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_DPRES, 0);
  */
 PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0);
 
-/**
- * subscription fuel tank
- *
- * Enable UAVCAN fuel tank status subscription.
- *
- * @boolean
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(UAVCAN_SUB_FUEL, 0);
-
 /**
  * subscription GPS
  *

src/examples/fake_magnetometer/FakeMagnetometer.cpp

@@ -66,12 +66,15 @@ void FakeMagnetometer::Run()
 		if (_vehicle_gps_position_sub.copy(&gps)) {
 			if (gps.eph < 1000) {
 
+				const double lat = gps.latitude_deg;
+				const double lon = gps.longitude_deg;
+
 				// magnetic field data returned by the geo library using the current GPS position
-				const float declination_rad = math::radians(get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg));
-				const float inclination_rad = math::radians(get_mag_inclination_degrees(gps.latitude_deg, gps.longitude_deg));
-				const float field_strength_gauss = get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);
+				const float mag_declination_gps = get_mag_declination_radians(lat, lon);
+				const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+				const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
 
-				_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);
+				_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
 
 				_mag_earth_available = true;
 			}

src/lib/battery/battery.cpp

@@ -305,27 +305,19 @@ void Battery::computeScale()
 float Battery::computeRemainingTime(float current_a)
 {
 	float time_remaining_s = NAN;
-	bool reset_current_avg_filter = false;
 
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;
 
 		if (_vehicle_status_sub.copy(&vehicle_status)) {
 			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-
-			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) {
-				reset_current_avg_filter = true;
-			}
-
 			_vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
 		}
 	}
 
 	_flight_phase_estimation_sub.update();
 
-	// reset filter if not feasible, negative or we did a VTOL transition to FW mode
-	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON
-	    || reset_current_avg_filter) {
+	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) {
 		_current_average_filter_a.reset(_params.bat_avrg_current);
 	}