Paparazzi UAS  v6.2.0_stable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_heater.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "imu_heater.h"
28 #include "mcu_periph/sys_time.h"
29 #include "modules/core/abi.h"
30 #include "modules/imu/imu.h"
31 #include "math/pprz_random.h"
32 
33 #if INTERMCU_IOMCU
34 #include "modules/intermcu/iomcu.h"
35 #endif
36 
38 #ifndef IMU_HEATER_TARGET_TEMP
39 #define IMU_HEATER_TARGET_TEMP 45
40 #endif
41 
43 #ifndef IMU_HEATER_P_GAIN
44 #define IMU_HEATER_P_GAIN 200.0
45 #endif
46 
48 #ifndef IMU_HEATER_I_GAIN
49 #define IMU_HEATER_I_GAIN 0.3
50 #endif
51 
53 #if !defined(IMU_HEATER_GYRO_ID) && !defined(IMU_HEATER_ACCEL_ID)
54 #define IMU_HEATER_GYRO_ID ABI_BROADCAST
55 #endif
56 
60 static float imu_heater_run(struct imu_heater_t *heater, float temp);
61 
62 #if PERIODIC_TELEMETRY
64 
71 static void send_imu_heater(struct transport_tx *trans, struct link_device *dev)
72 {
73  pprz_msg_send_IMU_HEATER(trans, dev, AC_ID, &imu_heater.meas_temp, &imu_heater.target_temp, &imu_heater.heat_cmd);
74 }
75 #endif
76 
77 #if defined(IMU_HEATER_GYRO_ID)
78 static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Rates *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) {
79  if(isnan(temp))
80  return;
81 
82  imu_heater.meas_temp_sum += temp;
84 }
85 #endif
86 
87 #if defined(IMU_HEATER_ACCEL_ID)
88 static void imu_heater_accel_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) {
89  if(isnan(temp))
90  return;
91 
92  imu_heater.meas_temp_sum += temp;
94 }
95 #endif
96 
101 void imu_heater_init(void)
102 {
103  /* Setup the heater */
107  imu_heater.meas_temp = NAN;
111  imu_heater.last_ts = 0;
112  imu_heater.heat_cmd = 0;
113 
114  /* Bind to raw temperature measurements */
115 #if defined(IMU_HEATER_GYRO_ID)
117 #elif defined(IMU_HEATER_ACCEL_ID)
118  AbiBindMsgIMU_ACCEL_RAW(IMU_HEATER_ACCEL_ID, &imu_heater_abi_ev, imu_heater_accel_raw_cb);
119 #endif
120 
121 #if PERIODIC_TELEMETRY
123 #endif
124 }
125 
133 {
134  // Set the GPIO heating resistor (using random noise for the magnetometer)
135 #if defined(IMU_HEATER_GPIO) && defined(IMU_HEATER_GPIO_PIN)
136  if (imu_heater.heat_cmd != 0 && rand_uniform() * 100 <= imu_heater.heat_cmd) {
137  gpio_set(IMU_HEATER_GPIO, IMU_HEATER_GPIO_PIN);
138  } else {
139  gpio_clear(IMU_HEATER_GPIO, IMU_HEATER_GPIO_PIN);
140  }
141 #endif
142 }
143 
150 {
151  // Calculate the command if we have measurements
152  if (imu_heater.meas_temp_cnt > 0) {
155 
158  }
159 
160  // Send to the IOMCU if needed
161 #if INTERMCU_IOMCU
163 #endif
164 }
165 
173 static float imu_heater_run(struct imu_heater_t *heater, float temp)
174 {
175  if (isnan(temp) || heater->target_temp == -1) {
176  return 0;
177  }
178 
179  // Get the delta time
180  uint32_t current_ts = get_sys_time_usec();
181  float dt = (current_ts - heater->last_ts) * 1e-6;
182 
183  // Calculate the error
184  float error = heater->target_temp - temp;
185 
186  // Only do integration if dt is correct
187  if (heater->last_ts != 0 && dt > 0) {
188  // Calculate the integrated value and bound to 70%
189  heater->integrated += heater->gain_i * error * dt;
190  BoundAbs(heater->integrated, 70);
191  }
192  heater->last_ts = current_ts;
193 
194  // Return the command percentage (0-100% bounded)
195  float cmd = error * heater->gain_p + heater->integrated;
196  Bound(cmd, 0, 100);
197  return cmd;
198 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:108
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
angular rates
Inertial Measurement Unit interface.
#define IMU_HEATER_P_GAIN
Default heater kP gain.
Definition: imu_heater.c:44
#define IMU_HEATER_GYRO_ID
Default heater gyro abi ID.
Definition: imu_heater.c:54
#define IMU_HEATER_TARGET_TEMP
Default target temperature.
Definition: imu_heater.c:39
static float imu_heater_run(struct imu_heater_t *heater, float temp)
Calculate the IMU command percentage.
Definition: imu_heater.c:173
static void send_imu_heater(struct transport_tx *trans, struct link_device *dev)
Send the IMU heater message.
Definition: imu_heater.c:71
struct imu_heater_t imu_heater
Local variables and functions.
Definition: imu_heater.c:58
static abi_event imu_heater_abi_ev
Definition: imu_heater.c:59
static void imu_heater_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
Definition: imu_heater.c:78
void imu_heater_periodic_10hz(void)
10Hz IMU heater periodic This will run the control loop for the IMU heater and in case of an IOMCU wi...
Definition: imu_heater.c:149
void imu_heater_periodic(void)
High speed heater periodic This calculates the temperature average and in case of GPIO heater it will...
Definition: imu_heater.c:132
#define IMU_HEATER_I_GAIN
Default heater kI gain.
Definition: imu_heater.c:49
void imu_heater_init(void)
Initialize the heater Sets the default gains and resets the initial values.
Definition: imu_heater.c:101
IMU heater module which can actuate a resistor heater through GPIO or IOMCU.
uint32_t meas_temp_cnt
Amount of summed temperatures.
Definition: imu_heater.h:37
float meas_temp
Measered average temperature in degrees Celcius.
Definition: imu_heater.h:35
uint8_t heat_cmd
Heater command 0-100%.
Definition: imu_heater.h:42
float target_temp
Target temeperature in degrees Celcius.
Definition: imu_heater.h:34
uint32_t last_ts
Last integration timestamp.
Definition: imu_heater.h:41
float gain_p
Heater kP gain.
Definition: imu_heater.h:38
float meas_temp_sum
Summed temperature in degrees Celcius.
Definition: imu_heater.h:36
float integrated
Integrated temperature error multiplied by kI (max 70%)
Definition: imu_heater.h:40
float gain_i
Heater kI gain.
Definition: imu_heater.h:39
Main IMU heater structure.
Definition: imu_heater.h:33
void iomcu_set_heater_duty_cycle(uint8_t duty_cycle)
Set the IO MCU heater duty cycle.
Definition: iomcu.c:274
Driver to communicate with the ardupilot IO MCU.
double rand_uniform(void)
Definition: pprz_random.c:45
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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