diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index 65f09bb53f..a4615679cf 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -157,7 +157,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @Range: -1 80 // @Units: degC // @User: Advanced - AP_GROUPINFO("IMU_TARGTEMP", 8, AP_BoardConfig, _imu_target_temperature, HAL_IMU_TEMP_DEFAULT), + AP_GROUPINFO("IMU_TARGTEMP", 8, AP_BoardConfig, heater.imu_target_temperature, HAL_IMU_TEMP_DEFAULT), #endif #if AP_FEATURE_BOARD_DETECT @@ -255,6 +255,30 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = { // @Units: ms // @User: Advanced AP_GROUPINFO("BOOT_DELAY", 20, AP_BoardConfig, _boot_delay_ms, HAL_DEFAULT_BOOT_DELAY), + +#if HAL_HAVE_IMU_HEATER + // @Param: IMUHEAT_P + // @DisplayName: IMU Heater P gain + // @Description: IMU Heater P gain + // @Range: 1 500 + // @Increment: 1 + // @User: Advanced + + // @Param: IMUHEAT_I + // @DisplayName: IMU Heater I gain + // @Description: IMU Heater integrator gain + // @Range: 0 1 + // @Increment: 0.1 + // @User: Advanced + + // @Param: IMUHEAT_IMAX + // @DisplayName: IMU Heater IMAX + // @Description: IMU Heater integrator maximum + // @Range: 0 100 + // @Increment: 1 + // @User: Advanced + AP_SUBGROUPINFO(heater.pi_controller, "IMUHEAT_", 21, AP_BoardConfig, AC_PI), +#endif AP_GROUPEND }; @@ -263,13 +287,6 @@ void AP_BoardConfig::init() { board_setup(); -#if HAL_HAVE_IMU_HEATER - // let the HAL know the target temperature. We pass a pointer as - // we want the user to be able to change the parameter without - // rebooting - hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature); -#endif - AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW); if (_boot_delay_ms > 0) { @@ -373,3 +390,9 @@ bool AP_BoardConfig::safety_button_handle_pressed(uint8_t press_count) } return true; } + +namespace AP { + AP_BoardConfig *boardConfig(void) { + return AP_BoardConfig::get_singleton(); + } +}; diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 9ff47cf414..8f63fe3d61 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -4,6 +4,7 @@ #include #include #include +#include #ifndef AP_FEATURE_BOARD_DETECT #if defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_CHIBIOS_ARCH_FMUV4PRO) || defined(HAL_CHIBIOS_ARCH_BRAINV51) || defined(HAL_CHIBIOS_ARCH_BRAINV52) || defined(HAL_CHIBIOS_ARCH_UBRAINV51) || defined(HAL_CHIBIOS_ARCH_COREV10) || defined(HAL_CHIBIOS_ARCH_BRAINV54) @@ -173,6 +174,10 @@ public: // should be toggled bool safety_button_handle_pressed(uint8_t press_count); +#if HAL_HAVE_IMU_HEATER + void set_imu_temp(float current_temp_c); +#endif + private: static AP_BoardConfig *_singleton; @@ -211,8 +216,17 @@ private: static bool _in_sensor_config_error; - // target temperarure for IMU in Celsius, or -1 to disable - AP_Int8 _imu_target_temperature; +#if HAL_HAVE_IMU_HEATER + struct { + AP_Int8 imu_target_temperature; + uint32_t last_update_ms; + AC_PI pi_controller{200, 0.3, 70}; + uint16_t count; + float sum; + float output; + uint32_t last_log_ms; + } heater; +#endif #if HAL_RCINPUT_WITH_AP_RADIO // direct attached radio @@ -242,3 +256,7 @@ private: AP_Int32 _options; }; + +namespace AP { + AP_BoardConfig *boardConfig(void); +}; diff --git a/libraries/AP_BoardConfig/IMU_heater.cpp b/libraries/AP_BoardConfig/IMU_heater.cpp new file mode 100644 index 0000000000..38790a95de --- /dev/null +++ b/libraries/AP_BoardConfig/IMU_heater.cpp @@ -0,0 +1,103 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + control IMU heater + */ + +#include +#include +#include +#include +#include +#include "AP_BoardConfig.h" + +#if HAL_HAVE_IMU_HEATER + +extern const AP_HAL::HAL& hal; + +void AP_BoardConfig::set_imu_temp(float current) +{ + int8_t target = heater.imu_target_temperature.get(); + // pass to HAL for Linux + hal.util->set_imu_target_temp((int8_t *)&heater.imu_target_temperature); + hal.util->set_imu_temp(current); + + if (target == -1) { + // nothing to do + return; + } + + + // limit to 65 degrees to prevent damage + target = constrain_int16(target, -1, 65); + + // average over temperatures to remove noise + heater.count++; + heater.sum += current; + + // update at 10Hz + uint32_t now = AP_HAL::millis(); + if (now - heater.last_update_ms < 100) { +#if defined(HAL_HEATER_GPIO_PIN) + // output as duty cycle to local pin. Use a random sequence to + // prevent a periodic change to magnetic field + bool heater_on = (get_random16() < uint32_t(heater.output) * 0xFFFFU / 100U); + hal.gpio->write(HAL_HEATER_GPIO_PIN, heater_on); +#endif + return; + } + float dt = (now - heater.last_update_ms) * 0.001; + dt = constrain_float(dt, 0, 0.5); + + heater.last_update_ms = now; + + float avg = heater.sum / heater.count; + heater.sum = 0; + heater.count = 0; + + if (target < 0) { + heater.output = 0; + } else { + heater.output = heater.pi_controller.update(avg, target, dt); + heater.output = constrain_float(heater.output, 0, 100); + } + + if (now - heater.last_log_ms >= 1000) { + AP::logger().Write("HEAT", "TimeUS,Temp,Targ,P,I,Out", "Qfbfff", + AP_HAL::micros64(), + avg, target, + heater.pi_controller.get_P(), + heater.pi_controller.get_I(), + heater.output); + heater.last_log_ms = now; + } +#if 0 + gcs().send_text(MAV_SEVERITY_INFO, "Heater: P=%.1f I=%.1f Out=%.1f Temp=%.1f", + heater.output, + avg); +#endif + +#if HAL_WITH_IO_MCU + if (io_enabled()) { + AP_IOMCU *iomcu = AP::iomcu(); + if (iomcu) { + // tell IOMCU to setup heater + iomcu->set_heater_duty_cycle(heater.output); + } + } +#endif +} + +#endif // HAL_HAVE_IMU_HEATER diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index 25f9cef358..c331618980 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -78,15 +78,17 @@ void AP_BoardConfig::board_setup_drivers(void) // run board auto-detection board_autodetect(); +#if HAL_HAVE_IMU_HEATER if (state.board_type == PX4_BOARD_PH2SLIM || state.board_type == PX4_BOARD_PIXHAWK2) { - _imu_target_temperature.set_default(45); - if (_imu_target_temperature.get() < 0) { + heater.imu_target_temperature.set_default(45); + if (heater.imu_target_temperature.get() < 0) { // don't allow a value of -1 on the cube, or it could cook // the IMU - _imu_target_temperature.set(45); + heater.imu_target_temperature.set(45); } } +#endif px4_configured_board = (enum px4_board_type)state.board_type.get();