AP_HAL_Linux: add support for a pwm heater

It uses a heating resistor controlled by a pwm.
By changing the duty cycle of the pwm, we can control the temperature.
A simple PI algorithm is used in order to get to the correct temperature
fast enough and without too much overshoot
It is implemented as a member of the Util class in order not to make to much
modification to the current codebase

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__

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__
 

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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HEAT_H__
+#define __HEAT_H__
+
+class Linux::LinuxHeat {
+public:
+    virtual void set_imu_temp(float current) { }
+};
+#endif

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <linux/limits.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+#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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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

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
 */

libraries/AP_HAL_Linux/Util.h

@@ -12,12 +12,7 @@ public:
         return static_cast<LinuxUtil*>(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;