Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
imu_temp_ctrl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 The Paparazzi team
3  * based on code from the Ardupilot project
4  *
5  * This file is part of paparazzi.
6  *
7  * This program is free software: you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation, either version 3 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20 
31 #include <stdio.h>
32 #include <unistd.h>
33 #include <fcntl.h>
34 
35 #include "std.h"
36 #include "mcu_periph/uart.h"
37 #include "pprzlink/messages.h"
39 #include "imu_temp_ctrl.h"
40 
43 
44 #ifdef BEBOP_VERSION2
45 #define PWM_HEAT_CHAN PWM_HEAT_CHAN_BEBOP2
46 #elif BOARD==disco
47 #define PWM_HEAT_CHAN PWM_HEAT_CHAN_DISCO
48 #else
49 #error temp control only implemented in Parrot Bebop2 and Disco
50 #endif
51 
53 {
54  float temp_current, error;
55  static float sum_error = 0;
56  uint32_t output = 0;
57 
58  temp_current = imu_bebop.mpu.temp;
59 
60  if (imu_temp_ctrl_ok == 1) {
61  /* minimal PI algo without dt from Ardupilot */
62  error = (float) ((IMU_TEMP_TARGET) - temp_current);
63 
64  /* Don't accumulate errors if the integrated error is superior
65  * to the max duty cycle(pwm_period)
66  */
67  if ((fabsf(sum_error) * IMU_TEMP_CTRL_KI < IMU_TEMP_CTRL_DUTY_MAX)) {
68  sum_error = sum_error + error;
69  }
70 
71  output = IMU_TEMP_CTRL_KP * error + IMU_TEMP_CTRL_KI * sum_error;
72 
73  if (output > IMU_TEMP_CTRL_DUTY_MAX) {
74  output = IMU_TEMP_CTRL_DUTY_MAX;
75  } else if (output < 0) {
76  output = 0;
77  }
78 
79  if (dprintf(pwm_heat_duty_fd, "%u", output) < 0) {
80  printf("[temp-ctrl] could not set duty cycle\n");
81  }
82  }
83 
84 #ifdef IMU_TEMP_CTRL_DEBUG
85  uint16_t duty_cycle;
86  duty_cycle = (uint16_t) ((uint32_t) output / (IMU_TEMP_CTRL_DUTY_MAX/100));
87 
88  RunOnceEvery(IMU_TEMP_CTRL_PERIODIC_FREQ, DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &duty_cycle, &temp_current));
89 #endif
90 }
91 
93 {
94  int pwm_heat_run_fd, ret;
95  char* path;
96 
97  ret = asprintf(&path, "/sys/class/pwm/pwm_%u/run", PWM_HEAT_CHAN);
98  if (ret < 0) {
99  printf("[temp-ctrl] could not create pwm path\n");
100  return;
101  }
102 
103  pwm_heat_run_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
104  free(path);
105  if (pwm_heat_run_fd < 0) {
106  printf("[temp-ctrl] could not open pwm run device\n");
107  return;
108  }
109 
110  ret = asprintf(&path, "/sys/class/pwm/pwm_%u/duty_ns", PWM_HEAT_CHAN);
111  if (ret < 0) {
112  printf("[temp-ctrl] could not create pwm duty path\n");
113  close(pwm_heat_run_fd);
114  return;
115  }
116 
117  pwm_heat_duty_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
118  free(path);
119  if (pwm_heat_duty_fd < 0) {
120  printf("[temp-ctrl] could not open pwm duty path\n");
121  close(pwm_heat_run_fd);
122  return;
123  }
124 
125  ret = write(pwm_heat_duty_fd, "0", 1);
126  if (ret != 1) {
127  printf("[temp-ctrl] could not set duty cycle\n");
128  goto error;
129  }
130 
131  ret = write(pwm_heat_run_fd, "0", 1);
132  if (ret != 1) {
133  printf("[temp-ctrl] could not disable pwm\n");
134  goto error;
135  }
136 
137  ret = write(pwm_heat_run_fd, "1", 1);
138  if (ret != 1) {
139  printf("[temp-ctrl] could not enable pwm\n");
140  goto error;
141  }
142 
143  imu_temp_ctrl_ok = 1;
144  return;
145 
146 error:
147  close(pwm_heat_run_fd);
148  close(pwm_heat_duty_fd);
149 }
struct ImuBebop imu_bebop
Basic Navstik IMU data.
Definition: imu_bebop.c:71
struct Mpu60x0_I2c mpu
The MPU gyro/accel device.
Definition: imu_bebop.h:47
void imu_temp_ctrl_periodic(void)
Definition: imu_temp_ctrl.c:52
void imu_temp_ctrl_init(void)
Definition: imu_temp_ctrl.c:92
#define PWM_HEAT_CHAN
Definition: imu_temp_ctrl.c:47
int pwm_heat_duty_fd
Definition: imu_temp_ctrl.c:42
uint8_t imu_temp_ctrl_ok
Definition: imu_temp_ctrl.c:41
#define IMU_TEMP_CTRL_KP
Definition: imu_temp_ctrl.h:45
#define IMU_TEMP_CTRL_KI
Definition: imu_temp_ctrl.h:46
#define IMU_TEMP_CTRL_DUTY_MAX
Definition: imu_temp_ctrl.h:39
#define IMU_TEMP_TARGET
Definition: imu_temp_ctrl.h:42
float temp
temperature in degrees Celcius
Definition: mpu60x0_i2c.h:66
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98