diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index e9dc9bcc6f..7b321c957f 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -40,6 +40,8 @@ namespace Linux { class LinuxUtil; class LinuxUtilRPI; class ToneAlarm; //limit the scope of ToneAlarm driver to Linux_HAL only + class LinuxHeat; + class LinuxHeatPwm; } #endif // __AP_HAL_LINUX_NAMESPACE_H__ diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index c332e43069..2cdc2ad34d 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -32,6 +32,8 @@ #include "ToneAlarmDriver.h" #include "Util.h" #include "Util_RPI.h" +#include "Heat.h" +#include "Heat_Pwm.h" #endif // __AP_HAL_LINUX_PRIVATE_H__ diff --git a/libraries/AP_HAL_Linux/Heat.h b/libraries/AP_HAL_Linux/Heat.h new file mode 100644 index 0000000000..bf88500095 --- /dev/null +++ b/libraries/AP_HAL_Linux/Heat.h @@ -0,0 +1,24 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#ifndef __HEAT_H__ +#define __HEAT_H__ + +class Linux::LinuxHeat { +public: + virtual void set_imu_temp(float current) { } +}; +#endif diff --git a/libraries/AP_HAL_Linux/Heat_Pwm.cpp b/libraries/AP_HAL_Linux/Heat_Pwm.cpp new file mode 100644 index 0000000000..6193dc6c82 --- /dev/null +++ b/libraries/AP_HAL_Linux/Heat_Pwm.cpp @@ -0,0 +1,139 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Heat_Pwm.h" + +extern const AP_HAL::HAL& hal; + +#define HEAT_PWM_DUTY "duty_ns" +#define HEAT_PWM_PERIOD "period_ns" +#define HEAT_PWM_RUN "run" + +using namespace Linux; + +/* + * Constructor : + * argument : pwm_sysfs_path is the path to the pwm directory, + * i.e /sys/class/pwm/pwm_6 on the bebop + */ +LinuxHeatPwm::LinuxHeatPwm(const char* pwm_sysfs_path, float Kp, float Ki, uint32_t period_ns, float target) : + _Kp(Kp), + _Ki(Ki), + _period_ns(period_ns), + _target(target) +{ + char *duty_path; + char *period_path; + char *run_path; + + if (asprintf(&duty_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_DUTY) == -1) { + hal.scheduler->panic("HeatPwm not enough memory\n"); + } + _duty_fd = open(duty_path, O_RDWR); + if (_duty_fd == -1) { + perror("opening duty"); + hal.scheduler->panic("Error Initializing Pwm heat\n"); + } + free(duty_path); + + if (asprintf(&period_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_PERIOD) == -1) { + hal.scheduler->panic("HeatPwm not enough memory\n"); + } + _period_fd = open(period_path, O_RDWR); + if (_period_fd == -1) { + perror("opening period"); + hal.scheduler->panic("Error Initializing Pwm heat\n"); + } + free(period_path); + + if (asprintf(&run_path, "%s/%s", pwm_sysfs_path, HEAT_PWM_RUN) == -1) { + hal.scheduler->panic("HeatPwm not enough memory\n"); + } + _run_fd = open(run_path, O_RDWR); + if (_run_fd == -1) { + perror("opening run"); + hal.scheduler->panic("Error Initializing Pwm heat\n"); + } + free(run_path); + + _set_period(_period_ns); + _set_duty(0); + _set_run(); +} + +void LinuxHeatPwm::set_imu_temp(float current) +{ + float error, output; + + if (hal.scheduler->millis() - _last_temp_update < 5) { + return; + } + + /* minimal PI algo without dt */ + error = _target - current; + /* Don't accumulate errors if the integrated error is superior + * to the max duty cycle(pwm_period) + */ + if ((fabsf(_sum_error) * _Ki < _period_ns)) { + _sum_error = _sum_error + error; + } + + output = _Kp*error + _Ki * _sum_error; + + if (output > _period_ns) { + output = _period_ns; + } else if (output < 0) { + output = 0; + } + + _set_duty(output); + _last_temp_update = hal.scheduler->millis(); +} + +void LinuxHeatPwm::_set_duty(uint32_t duty) +{ + if (dprintf(_duty_fd, "0x%x", duty) < 0) { + perror("pwm set_duty"); + } +} + +void LinuxHeatPwm::_set_period(uint32_t period) +{ + if (dprintf(_period_fd, "0x%x", period) < 0) { + perror("pwm set_period"); + } +} + +void LinuxHeatPwm::_set_run() +{ + if (dprintf(_run_fd, "1") < 0) { + perror("pwm set_run"); + } +} + +#endif diff --git a/libraries/AP_HAL_Linux/Heat_Pwm.h b/libraries/AP_HAL_Linux/Heat_Pwm.h new file mode 100644 index 0000000000..e7a6ba445f --- /dev/null +++ b/libraries/AP_HAL_Linux/Heat_Pwm.h @@ -0,0 +1,43 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +#ifndef __HEAT_PWM_H__ +#define __HEAT_PWM_H__ + +#include "AP_HAL_Linux.h" +#include "Heat.h" + +class Linux::LinuxHeatPwm : public Linux::LinuxHeat { +public: + LinuxHeatPwm(const char* pwm_sysfs_path, float Kp, float Ki,uint32_t period_ns, float target); + void set_imu_temp(float current)override; + +private: + int _duty_fd = -1; + int _period_fd = -1; + int _run_fd = -1; + uint32_t _last_temp_update = 0; + float _Kp; + float _Ki; + uint32_t _period_ns; + float _sum_error; + float _target; + + void _set_duty(uint32_t duty); + void _set_period(uint32_t period); + void _set_run(); +}; +#endif diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 669e28692d..af4607023a 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -11,11 +11,38 @@ extern const AP_HAL::HAL& hal; #include "Util.h" +#include "Heat_Pwm.h" + using namespace Linux; static int state; ToneAlarm LinuxUtil::_toneAlarm; + +void LinuxUtil::init(int argc, char * const *argv) { + saved_argc = argc; + saved_argv = argv; + +#ifdef HAL_UTILS_HEAT +#if HAL_UTILS_HEAT == HAL_LINUX_HEAT_PWM + _heat = new Linux::LinuxHeatPwm(HAL_LINUX_HEAT_PWM_SYSFS_DIR, + HAL_LINUX_HEAT_KP, + HAL_LINUX_HEAT_KI, + HAL_LINUX_HEAT_PERIOD_NS, + HAL_LINUX_HEAT_TARGET_TEMP); +#else + #error Unrecognized Heat +#endif // #if +#else + _heat = new Linux::LinuxHeat(); +#endif // #ifdef +} + +void LinuxUtil::set_imu_temp(float current) +{ + _heat->set_imu_temp(current); +} + /** return commandline arguments, if available */ diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index fb88f1d1fe..e97752ab4d 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -12,12 +12,7 @@ public: return static_cast(util); } - void init(int argc, char * const *argv) { - saved_argc = argc; - saved_argv = argv; - } - - + void init(int argc, char * const *argv); bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } /** @@ -41,9 +36,11 @@ 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); private: static Linux::ToneAlarm _toneAlarm; + Linux::LinuxHeat *_heat; int saved_argc; char* const *saved_argv; const char* custom_log_directory = NULL;