fts module implementation build ok

msg/CMakeLists.txt

@@ -88,7 +88,7 @@ set(msg_files
 	FollowTarget.msg
 	FollowTargetEstimator.msg
 	FollowTargetStatus.msg
-	FTSStatus.msg
+	FtsStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GimbalDeviceAttitudeStatus.msg

src/drivers/fts/fts.cpp

@@ -24,7 +24,7 @@ enum IO_HEATER_MODE {
 };
 
 // Use direct calls to turn GPIO pin on/off
-#  if not defined(GPIO_FTS_CAN_OUTPUT) || if not defined(GPIO_FTS_CHUTE_OUTPUT)
+#if !defined(GPIO_FTS_CAN_OUTPUT) || !defined(GPIO_FTS_CHUTE_OUTPUT)
 #  error "To use the fts driver, the board_config.h must define and initialize GPIO_FTS_CAN_OUTPUT and GPIO_FTS_CHUTE_OUTPUT "
 #  endif
 
@@ -79,24 +79,7 @@ void FTS::fts_on()
 
 bool FTS::initialize_topics()
 {
-	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-
-		if (sensor_accel_sub.get().timestamp != 0 &&
-		    sensor_accel_sub.get().device_id != 0 &&
-		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
-
-			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
-			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
-				_sensor_accel_sub.ChangeInstance(i);
-				_sensor_device_id = sensor_accel_sub.get().device_id;
-				initialize_fts_io();
-				return true;
-			}
-		}
-	}
-
-	return false;
+	return true;
 }
 
 void FTS::Run()
@@ -108,53 +91,6 @@ void FTS::Run()
 
 	update_params();
 
-	if (_sensor_device_id == 0) {
-		if (!initialize_topics()) {
-			// if sensor still not found try again in 1 second
-			ScheduleDelayed(1_s);
-			return;
-		}
-	}
-
-	sensor_accel_s sensor_accel;
-	float temperature_delta {0.f};
-
-	if (_fts_on) {
-		// Turn the fts off.
-		_fts_on = false;
-		fts_off();
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
-
-	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-
-		// Update the current IMU sensor temperature if valid.
-		if (PX4_ISFINITE(sensor_accel.temperature)) {
-			temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature;
-			_temperature_last = sensor_accel.temperature;
-		}
-
-		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
-		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
-
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
-
-		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
-					   _integrator_value) * static_cast<float>(_controller_period_usec));
-
-		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
-
-		if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
-			_temperature_target_met = true;
-
-		} else {
-
-			_temperature_target_met = false;
-		}
-
-		_fts_on = true;
-		fts_on();
-		ScheduleDelayed(_controller_time_on_usec);
-	}
 
 	publish_status();
 }
@@ -169,15 +105,7 @@ void FTS::publish_status()
 
 int FTS::start()
 {
-	// Exit the driver if the sensor ID does not match the desired sensor.
-	if (_param_sens_temp_id.get() == 0) {
-		PX4_ERR("Valid SENS_TEMP_ID required");
-		request_stop();
-		return PX4_ERROR;
-	}
-
 	update_params(true);
-	ScheduleNow();
 	return PX4_OK;
 }
 
@@ -215,14 +143,6 @@ int FTS::print_usage(const char *reason)
 		printf("%s\n\n", reason);
 	}
 
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
-
-This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI.
-)DESCR_STR");
-
 	PRINT_MODULE_USAGE_NAME("fts", "system");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

src/drivers/fts/fts.h

@@ -79,7 +79,9 @@ private:
 
 	void publish_status();
 
-	/**
+    void Run() override;
+
+    /**
 	 * @brief Updates and checks for updated uORB parameters.
 	 * @param force Boolean to determine if an update check should be forced.
 	 */