diff --git a/libraries/AP_HAL_Linux/Heat.h b/libraries/AP_HAL_Linux/Heat.h index 3a99b3226a..adc7b4dee7 100644 --- a/libraries/AP_HAL_Linux/Heat.h +++ b/libraries/AP_HAL_Linux/Heat.h @@ -18,4 +18,5 @@ class Linux::Heat { public: virtual void set_imu_temp(float current) { } + virtual void set_imu_target_temp(int8_t *target) { } }; diff --git a/libraries/AP_HAL_Linux/Heat_Pwm.cpp b/libraries/AP_HAL_Linux/Heat_Pwm.cpp index 976f9fccd7..d659b24611 100644 --- a/libraries/AP_HAL_Linux/Heat_Pwm.cpp +++ b/libraries/AP_HAL_Linux/Heat_Pwm.cpp @@ -33,11 +33,10 @@ extern const AP_HAL::HAL& hal; using namespace Linux; -HeatPwm::HeatPwm(uint8_t pwm_num, float Kp, float Ki, uint32_t period_ns, float target) : +HeatPwm::HeatPwm(uint8_t pwm_num, float Kp, float Ki, uint32_t period_ns) : _Kp(Kp), _Ki(Ki), - _period_ns(period_ns), - _target(target) + _period_ns(period_ns) { _pwm = new PWM_Sysfs_Bebop(pwm_num); _pwm->set_period(_period_ns); @@ -49,12 +48,17 @@ void HeatPwm::set_imu_temp(float current) { float error, output; + if (_target == nullptr) { + // not configured + return; + } + if (AP_HAL::millis() - _last_temp_update < 5) { return; } /* minimal PI algo without dt */ - error = _target - current; + error = ((float)*_target) - current; /* Don't accumulate errors if the integrated error is superior * to the max duty cycle(pwm_period) */ @@ -74,4 +78,9 @@ void HeatPwm::set_imu_temp(float current) _last_temp_update = AP_HAL::millis(); } +void HeatPwm::set_imu_target_temp(int8_t *target) +{ + _target = target; +} + #endif diff --git a/libraries/AP_HAL_Linux/Heat_Pwm.h b/libraries/AP_HAL_Linux/Heat_Pwm.h index 06ce946fc7..18bf5ee567 100644 --- a/libraries/AP_HAL_Linux/Heat_Pwm.h +++ b/libraries/AP_HAL_Linux/Heat_Pwm.h @@ -22,8 +22,9 @@ class Linux::HeatPwm : public Linux::Heat { public: HeatPwm(uint8_t pwm_num, float Kp, float Ki, - uint32_t period_ns, float target); - void set_imu_temp(float current)override; + uint32_t period_ns); + void set_imu_temp(float current) override; + void set_imu_target_temp(int8_t *target) override; private: PWM_Sysfs_Base *_pwm; @@ -32,5 +33,5 @@ private: float _Ki; uint32_t _period_ns; float _sum_error; - float _target; + int8_t *_target = nullptr; }; diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 793c118a5c..939528a281 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -31,10 +31,9 @@ void Util::init(int argc, char * const *argv) { #ifdef HAL_UTILS_HEAT #if HAL_UTILS_HEAT == HAL_LINUX_HEAT_PWM _heat = new Linux::HeatPwm(HAL_LINUX_HEAT_PWM_NUM, - HAL_LINUX_HEAT_KP, - HAL_LINUX_HEAT_KI, - HAL_LINUX_HEAT_PERIOD_NS, - HAL_LINUX_HEAT_TARGET_TEMP); + HAL_LINUX_HEAT_KP, + HAL_LINUX_HEAT_KI, + HAL_LINUX_HEAT_PERIOD_NS); #else #error Unrecognized Heat #endif // #if @@ -43,11 +42,18 @@ void Util::init(int argc, char * const *argv) { #endif // #ifdef } +// set current IMU temperatue in degrees C void Util::set_imu_temp(float current) { _heat->set_imu_temp(current); } +// set target IMU temperatue in degrees C +void Util::set_imu_target_temp(int8_t *target) +{ + _heat->set_imu_target_temp(target); +} + /** return commandline arguments, if available */ diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index cbf604a7cf..4596bd9274 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -46,7 +46,8 @@ public: void set_custom_terrain_directory(const char *_custom_terrain_directory) { custom_terrain_directory = _custom_terrain_directory; } bool is_chardev_node(const char *path); - void set_imu_temp(float current); + void set_imu_temp(float current) override; + void set_imu_target_temp(int8_t *target) override; uint32_t available_memory(void) override;