diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 64cfd389d4..25f4f0c734 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -68,6 +68,7 @@ set(msg_files geofence_result.msg gps_dump.msg gps_inject_data.msg + heater_status.msg home_position.msg hover_thrust_estimate.msg input_rc.msg @@ -127,11 +128,11 @@ set(msg_files sensor_selection.msg sensors_status_imu.msg system_power.msg + takeoff_status.msg task_stack_info.msg tecs_status.msg telemetry_status.msg test_motor.msg - takeoff_status.msg timesync.msg timesync_status.msg trajectory_bezier.msg diff --git a/msg/heater_status.msg b/msg/heater_status.msg new file mode 100644 index 0000000000..158d28fb99 --- /dev/null +++ b/msg/heater_status.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id + +bool heater_on + +float32 temperature_sensor +float32 temperature_target + +uint32 controller_period_usec +uint32 controller_time_on_usec + +float32 proportional_value +float32 integrator_value +float32 feed_forward_value + +uint8 MODE_GPIO = 1 +uint8 MODE_PX4IO = 2 +uint8 mode diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 070a61c5ba..9727a1886e 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -313,6 +313,8 @@ rtps: id: 148 - msg: takeoff_status id: 149 + - msg: heater_status + id: 150 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index f73cbfca08..aca5d55eca 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -73,17 +73,11 @@ Heater::Heater() : } #endif - - heater_enable(); } Heater::~Heater() { heater_disable(); - -#ifdef HEATER_PX4IO - px4_close(_io_fd); -#endif } int Heater::custom_command(int argc, char *argv[]) @@ -97,28 +91,35 @@ int Heater::custom_command(int argc, char *argv[]) return print_usage("Unrecognized command."); } -uint32_t Heater::get_sensor_id() -{ - return _sensor_accel.device_id; -} - void Heater::heater_disable() { // Reset heater to off state. #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); + } + #endif + #ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); #endif } -void Heater::heater_enable() +void Heater::heater_initialize() { // Initialize heater to off state. #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + if (_io_fd < 0) { + _io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR); + } + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + } + #endif + #ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); #endif @@ -127,8 +128,13 @@ void Heater::heater_enable() void Heater::heater_off() { #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + } + #endif + #ifdef HEATER_GPIO px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); #endif @@ -137,64 +143,170 @@ void Heater::heater_off() void Heater::heater_on() { #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); + } + #endif + #ifdef HEATER_GPIO px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); #endif } -void Heater::initialize_topics() +bool Heater::initialize_topics() { - // Get the total number of accelerometer instances. - uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel)); + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - // Get the total number of accelerometer instances and check each instance for the correct ID. - for (uint8_t x = 0; x < number_of_imus; x++) { - _sensor_accel.device_id = 0; + if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0 + && PX4_ISFINITE(sensor_accel_sub.get().temperature)) { - while (_sensor_accel.device_id == 0) { - _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; - - if (!_sensor_accel_sub.advertised()) { - px4_usleep(100); - continue; + // 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; + return true; } - - _sensor_accel_sub.copy(&_sensor_accel); - } - - // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. - if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) { - break; } } - // Exit the driver if the sensor ID does not match the desired sensor. - if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) { - request_stop(); - PX4_ERR("Could not identify IMU sensor."); - } + return false; } -int Heater::print_status() +void Heater::Run() { - float _feedforward_value = _param_sens_imu_temp_ff.get(); + if (should_exit()) { +#if defined(HEATER_PX4IO) - PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d", - _sensor_accel.device_id, - static_cast(_param_sens_imu_temp.get()), - static_cast(_sensor_accel.temperature), - _controller_period_usec); - PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%", - static_cast(_feedforward_value * 100), - static_cast(_proportional_value * 100), - static_cast(_integrator_value * 100), - static_cast(static_cast(_controller_time_on_usec) / static_cast(_controller_period_usec) * 100)); + // must be closed from wq thread + if (_io_fd >= 0) { + px4_close(_io_fd); + } +#endif + exit_and_cleanup(); + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + } + + if (_sensor_device_id == 0) { + if (initialize_topics()) { + heater_initialize(); + + } else { + // if sensor still not found try again in 1 second + ScheduleDelayed(1_s); + return; + } + } + + sensor_accel_s sensor_accel; + + if (_heater_on) { + // Turn the heater off. + heater_off(); + _heater_on = false; + + } else if (_sensor_accel_sub.update(&sensor_accel)) { + float temperature_delta {0.f}; + + // 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(); + + if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { + _integrator_value = 0.f; + } + + // Guard against integrator wind up. + _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); + + _heater_on = true; + heater_on(); + } + + // Schedule the next cycle. + if (_heater_on) { + ScheduleDelayed(_controller_time_on_usec); + + } else { + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); + } + + + // publish status + heater_status_s status{}; + status.heater_on = _heater_on; + status.device_id = _sensor_device_id; + status.temperature_sensor = _temperature_last; + status.temperature_target = _param_sens_imu_temp.get(); + status.controller_period_usec = _controller_period_usec; + status.controller_time_on_usec = _controller_time_on_usec; + status.proportional_value = _proportional_value; + status.integrator_value = _integrator_value; + status.feed_forward_value = _param_sens_imu_temp_ff.get(); + +#ifdef HEATER_PX4IO + status.mode |= heater_status_s::MODE_PX4IO; +#endif +#ifdef HEATER_GPIO + status.mode |= heater_status_s::MODE_GPIO; +#endif + + status.timestamp = hrt_absolute_time(); + _heater_status_pub.publish(status); +} + +int Heater::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; + } + + ScheduleNow(); return PX4_OK; } +int Heater::task_spawn(int argc, char *argv[]) +{ + Heater *heater = new Heater(); + + if (!heater) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(heater); + _task_id = task_id_is_work_queue; + + heater->start(); + return 0; +} + int Heater::print_usage(const char *reason) { if (reason) { @@ -216,101 +328,7 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE return 0; } -void Heater::Run() -{ - if (should_exit()) { - exit_and_cleanup(); - return; - } - - if (_heater_on) { - // Turn the heater off. - heater_off(); - _heater_on = false; - - } else { - update_params(false); - - _sensor_accel_sub.update(&_sensor_accel); - - float temperature_delta {0.f}; - - // Update the current IMU sensor temperature if valid. - if (!isnan(_sensor_accel.temperature)) { - temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature; - } - - _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); - _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); - - if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { - _integrator_value = 0.f; - } - - // Guard against integrator wind up. - _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); - - _heater_on = true; - heater_on(); - } - - // Schedule the next cycle. - if (_heater_on) { - ScheduleDelayed(_controller_time_on_usec); - - } else { - ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); - } -} - -int Heater::start() -{ - update_params(true); - initialize_topics(); - - // Allow sufficient time for all additional sensors and processes to start. - ScheduleDelayed(100000); - return PX4_OK; -} - -int Heater::task_spawn(int argc, char *argv[]) -{ - Heater *heater = new Heater(); - - if (!heater) { - PX4_ERR("driver allocation failed"); - return PX4_ERROR; - } - - _object.store(heater); - _task_id = task_id_is_work_queue; - - heater->start(); - return 0; -} - -void Heater::update_params(const bool force) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - ModuleParams::updateParams(); - } -} - -/** - * Main entry point for the heater driver module - */ -int heater_main(int argc, char *argv[]) +extern "C" __EXPORT int heater_main(int argc, char *argv[]) { return Heater::main(argc, argv); } diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 126287c452..37be610630 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -47,7 +47,9 @@ #include #include #include +#include #include +#include #include #include @@ -57,13 +59,6 @@ using namespace time_literals; #define CONTROLLER_PERIOD_DEFAULT 100000 -/** - * @brief IMU Heater Controller driver used to maintain consistent - * temparature at the IMU. - */ -extern "C" __EXPORT int heater_main(int argc, char *argv[]); - - class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -97,30 +92,6 @@ public: */ static int task_spawn(int argc, char *argv[]); - /** - * @brief Sets and/or reports the heater controller time period value in microseconds. - * @param argv Pointer to the input argument array. - * @return Returns 0 iff successful, -1 otherwise. - */ - int controller_period(char *argv[]); - - /** @brief Returns the id of the target sensor. */ - uint32_t get_sensor_id(); - - /** - * @brief Sets and/or reports the heater controller integrator gain value. - * @param argv Pointer to the input argument array. - * @return Returns the heater integrator gain value iff successful, 0.0f otherwise. - */ - float integrator(char *argv[]); - - /** - * @brief Sets and/or reports the heater controller proportional gain value. - * @param argv Pointer to the input argument array. - * @return Returns the heater proportional gain value iff successful, 0.0f otherwise. - */ - float proportional(char *argv[]); - /** * @brief Initiates the heater driver work queue, starts a new background task, * and fails if it is already running. @@ -128,25 +99,10 @@ public: */ int start(); - /** - * @brief Reports curent status and diagnostic information about the heater driver. - * @return Returns 0 iff successful, -1 otherwise. - */ - int print_status(); - - /** - * @brief Sets and/or reports the heater target temperature. - * @param argv Pointer to the input argument array. - * @return Returns the heater target temperature value iff successful, -1.0f otherwise. - */ - float temperature_setpoint(char *argv[]); - -protected: +private: /** @brief Called once to initialize uORB topics. */ - void initialize_topics(); - -private: + bool initialize_topics(); /** @brief Calculates the heater element on/off time and schedules the next cycle. */ void Run() override; @@ -158,7 +114,7 @@ private: void update_params(const bool force = false); /** Enables / configures the heater (either by GPIO or PX4IO). */ - void heater_enable(); + void heater_initialize(); /** Disnables the heater (either by GPIO or PX4IO). */ void heater_disable(); @@ -174,7 +130,7 @@ private: /** File descriptor for PX4IO for heater ioctl's */ #if defined(HEATER_PX4IO) - int _io_fd; + int _io_fd_ {-1}; #endif bool _heater_on = false; @@ -185,10 +141,15 @@ private: float _integrator_value = 0.0f; float _proportional_value = 0.0f; + uORB::Publication _heater_status_pub{ORB_ID(heater_status)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)}; - sensor_accel_s _sensor_accel{}; + + uint32_t _sensor_device_id{0}; + + float _temperature_last{NAN}; DEFINE_PARAMETERS( (ParamFloat) _param_sens_imu_temp_ff, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a0a6fa4118..ac427f9828 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics() add_topic("cpuload"); add_topic("esc_status", 250); add_topic("generator_status"); + add_topic("heater_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500);