diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7881e11..d7ff2e0 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/FTSStatus.msg b/msg/FtsStatus.msg similarity index 100% rename from msg/FTSStatus.msg rename to msg/FtsStatus.msg diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp index 0443d88..133a22c 100644 --- a/src/drivers/fts/fts.cpp +++ b/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_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((_param_sens_imu_temp_ff.get() + _proportional_value + - _integrator_value) * static_cast(_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(); diff --git a/src/drivers/fts/fts.h b/src/drivers/fts/fts.h index 1821f9b..b94ab64 100644 --- a/src/drivers/fts/fts.h +++ b/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. */