add temperature control module for Parrot Bebop2/Disco

It is possible to control the temperature of the MPU6xxx in the Bebop2 and Disco through resistors powered by PWM_6. Code mostly taken from Ardupilot Seems this does not make a big difference even if flying in cold conditions but does rather improve things than do harm. MPU temperature will go to about +15 deg C in a Bebop2 at 0 deg C ambient temperature. Having the gyro/accel calibrated at just below +50 deg C after running the original Parrot software creates a 30+ deg C change.
paparazzi · Mar 1, 2017 · 7848138 · 7848138
1 parent 356062f
commit 7848138
Show file tree

Hide file tree

Showing 3 changed files with 218 additions and 0 deletions.
diff --git a/conf/modules/imu_temp_ctrl.xml b/conf/modules/imu_temp_ctrl.xml
@@ -0,0 +1,18 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="imu_temp_ctrl" dir="ins">
+  <doc>
+    <description>
+      Bebop2/Disco INS (MPU6x) sensor temperature control.
+    </description>
+    <define name="IMU_TEMP_CTRL_DEBUG" description="output heater setting in percent and temperature in TMP_STATUS message"/>
+  </doc>
+  <header>
+    <file name="imu_temp_ctrl.h"/>
+  </header>
+  <init fun="imu_temp_ctrl_init()"/>
+  <periodic fun="imu_temp_ctrl_periodic()" freq="20."/>
+  <makefile target="ap">
+    <file name="imu_temp_ctrl.c"/>
+  </makefile>
+</module>
diff --git a/sw/airborne/modules/ins/imu_temp_ctrl.c b/sw/airborne/modules/ins/imu_temp_ctrl.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2017 The Paparazzi team
+ *                    based on code from the Ardupilot project
+ *
+ * This file is part of paparazzi.
+ *
+ * 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/>.
+ */
+
+/**
+ * @file modules/ins/imu_temp_ctrl.c
+ *
+ * INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO
+ *
+ * Controls the heating resistors in the Bebop2 to keep the MPU6050
+ * gyro/accel INS sensors at a constant temperature
+ *
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include "std.h"
+#include "mcu_periph/uart.h"
+#include "pprzlink/messages.h"
+#include "subsystems/datalink/downlink.h"
+#include "imu_temp_ctrl.h"
+
+uint8_t imu_temp_ctrl_ok = 0;
+int pwm_heat_duty_fd = 0;
+
+#ifdef BEBOP_VERSION2
+#define PWM_HEAT_CHAN PWM_HEAT_CHAN_BEBOP2
+#elif BOARD==disco
+#define PWM_HEAT_CHAN PWM_HEAT_CHAN_DISCO
+#else
+#error temp control only implemented in Parrot Bebop2 and Disco
+#endif
+
+void imu_temp_ctrl_periodic(void)
+{
+  float temp_current, error;
+  static float sum_error = 0;
+  uint32_t output = 0;
+
+  temp_current = imu_bebop.mpu.temp;
+
+  if (imu_temp_ctrl_ok == 1) {
+    /* minimal PI algo without dt from Ardupilot */
+    error = (float) ((IMU_TEMP_TARGET) - temp_current);
+
+    /* Don't accumulate errors if the integrated error is superior
+     * to the max duty cycle(pwm_period)
+     */
+    if ((fabsf(sum_error) * IMU_TEMP_CTRL_KI < IMU_TEMP_CTRL_DUTY_MAX)) {
+      sum_error = sum_error + error;
+    }
+
+    output = IMU_TEMP_CTRL_KP * error + IMU_TEMP_CTRL_KI * sum_error;
+
+    if (output > IMU_TEMP_CTRL_DUTY_MAX) {
+      output = IMU_TEMP_CTRL_DUTY_MAX;
+    } else if (output < 0) {
+      output = 0;
+    }
+
+    if (dprintf(pwm_heat_duty_fd, "%u", output) < 0) {
+      printf("[temp-ctrl] could not set duty cycle\n");
+    }
+  }
+
+#ifdef IMU_TEMP_CTRL_DEBUG
+  uint16_t duty_cycle;
+  duty_cycle = (uint16_t) ((uint32_t) output / (IMU_TEMP_CTRL_DUTY_MAX/100));
+
+  RunOnceEvery(IMU_TEMP_CTRL_PERIODIC_FREQ, DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &duty_cycle, &temp_current));
+#endif
+}
+
+void imu_temp_ctrl_init(void)
+{
+  int pwm_heat_run_fd, ret;
+  char* path;
+
+  ret = asprintf(&path, "/sys/class/pwm/pwm_%u/run", PWM_HEAT_CHAN);
+  if (ret < 0) {
+    printf("[temp-ctrl] could not create pwm path\n");
+    return;
+  }
+
+  pwm_heat_run_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+  free(path);
+  if (pwm_heat_run_fd < 0) {
+    printf("[temp-ctrl] could not open pwm run device\n");
+    return;
+  }
+
+  ret = asprintf(&path, "/sys/class/pwm/pwm_%u/duty_ns", PWM_HEAT_CHAN);
+  if (ret < 0) {
+    printf("[temp-ctrl] could not create pwm duty path\n");
+    close(pwm_heat_run_fd);
+    return;
+  }
+
+  pwm_heat_duty_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+  free(path);
+  if (pwm_heat_duty_fd < 0) {
+    printf("[temp-ctrl] could not open pwm duty path\n");
+    close(pwm_heat_run_fd);
+    return;
+  }
+
+  ret = write(pwm_heat_duty_fd, "0", 1);
+  if (ret != 1) {
+    printf("[temp-ctrl] could not set duty cycle\n");
+    goto error;
+  }
+
+  ret = write(pwm_heat_run_fd, "0", 1);
+  if (ret != 1) {
+    printf("[temp-ctrl] could not disable pwm\n");
+    goto error;
+  }
+
+  ret = write(pwm_heat_run_fd, "1", 1);
+  if (ret != 1) {
+    printf("[temp-ctrl] could not enable pwm\n");
+    goto error;
+  }
+
+  imu_temp_ctrl_ok = 1;
+  return;
+
+error:
+    close(pwm_heat_run_fd);
+    close(pwm_heat_duty_fd);
+}
diff --git a/sw/airborne/modules/ins/imu_temp_ctrl.h b/sw/airborne/modules/ins/imu_temp_ctrl.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2017 The Paparazzi team
+ *                    based on code from the Ardupilot project
+ *
+ * This file is part of paparazzi.
+ *
+ * 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/>.
+ */
+
+/**
+ * @file modules/ins/imu_temp_ctrl.c
+ *
+ * INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO
+ *
+ * Controls the heating resistors in the Bebop2 to keep the MPU6050
+ * gyro/accel INS sensors at a constant temperature
+ *
+ */
+
+#ifndef IMU_TEMP_CTRL_H
+#define IMU_TEMP_CTRL_H
+
+#include "std.h"
+
+void imu_temp_ctrl_init(void);
+void imu_temp_ctrl_periodic(void);
+
+#define IMU_TEMP_CTRL_DUTY_MAX 125000
+
+#ifndef IMU_TEMP_TARGET
+#define IMU_TEMP_TARGET 50
+#endif
+
+#define IMU_TEMP_CTRL_KP 20000
+#define IMU_TEMP_CTRL_KI 60
+
+#define PWM_HEAT_CHAN_BEBOP2 6
+#define PWM_HEAT_CHAN_DISCO 10
+
+#endif /* IMU_TEMP_CTRL_H */