Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
electrical.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2013 The Paparazzi Team
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 
29 
30 #include "mcu_periph/adc.h"
31 #include "modules/core/commands.h"
32 
33 #include "autopilot.h"
34 
35 #include "generated/airframe.h"
36 #include "generated/modules.h"
37 #include BOARD_CONFIG
38 
39 #ifdef MILLIAMP_PER_PERCENT
40 #warning "deprecated MILLIAMP_PER_PERCENT --> Please use MILLIAMP_AT_FULL_THROTTLE"
41 #endif
42 #if defined BATTERY_SENS || defined BATTERY_OFFSET
43 #warning "BATTERY_SENS and BATTERY_OFFSET are deprecated, please remove them --> if you want to change the default use VoltageOfAdc"
44 #endif
45 
46 #if defined COMMAND_THROTTLE
47 #define COMMAND_CURRENT_ESTIMATION COMMAND_THROTTLE
48 #elif defined COMMAND_THRUST
49 #define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST
50 #endif
51 
52 #ifndef BAT_CHECKER_DELAY
53 #define BAT_CHECKER_DELAY 5
54 #endif
55 
56 #ifndef MIN_BAT_LEVEL
57 #define MIN_BAT_LEVEL 3
58 #endif
59 
60 #ifndef TAKEOFF_BAT_LEVEL
61 #define TAKEOFF_BAT_LEVEL LOW_BAT_LEVEL
62 #endif
63 PRINT_CONFIG_VAR(TAKEOFF_BAT_LEVEL)
64 
65 PRINT_CONFIG_VAR(LOW_BAT_LEVEL)
66 PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL)
67 PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
68 
69 #ifndef VoltageOfAdc
70 #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
71 #endif
72 #ifndef VBoardOfAdc
73 #define VBoardOfAdc(adc) DefaultVBoardOfAdc(adc)
74 #endif
75 #ifndef MilliAmpereOfAdc
76 #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
77 #endif
78 
79 #ifndef CURRENT_ESTIMATION_NONLINEARITY
80 #define CURRENT_ESTIMATION_NONLINEARITY 1.2
81 #endif
82 PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
83 
84 #if defined MILLIAMP_AT_FULL_THROTTLE && !defined MILLIAMP_AT_IDLE_THROTTLE
85  PRINT_CONFIG_MSG("Assuming 0 mA at idle throttle")
86  #define MILLIAMP_AT_IDLE_THROTTLE 0
87 #endif
88 
89 PRINT_CONFIG_VAR(MILLIAMP_AT_IDLE_THROTTLE)
90 
91 /* Main external structure */
92 struct Electrical electrical;
93 
94 #if defined ADC_CHANNEL_VSUPPLY || (defined ADC_CHANNEL_CURRENT && !defined SITL) || defined MILLIAMP_AT_FULL_THROTTLE
95 static struct {
96 #ifdef ADC_CHANNEL_VSUPPLY
97  struct adc_buf vsupply_adc_buf;
98 #endif
99 #if defined ADC_CHANNEL_VBOARD
100  struct adc_buf vboard_adc_buf;
101 #endif
102 #if defined ADC_CHANNEL_CURRENT && !defined SITL
103  struct adc_buf current_adc_buf;
104 #endif
105 #if defined ADC_CHANNEL_CURRENT2 && !defined SITL
106  struct adc_buf current2_adc_buf;
107 #endif
108 #ifdef MILLIAMP_AT_FULL_THROTTLE
109  float nonlin_factor;
110 #endif
111 } electrical_priv;
112 #endif
113 
114 #ifdef PREFLIGHT_CHECKS
115 /* Preflight checks */
117 static struct preflight_check_t electrical_pfc;
118 
119 static void electrical_preflight(struct preflight_result_t *result) {
121  preflight_error(result, "Battery level %.2fV below minimum takeoff level %.2fV", electrical.vsupply, TAKEOFF_BAT_LEVEL);
122  } else {
123  preflight_success(result, "Battery level %.2fV above takeoff level %.2fV", electrical.vsupply, TAKEOFF_BAT_LEVEL);
124  }
125 }
126 #endif // PREFLIGHT_CHECKS
127 
128 void electrical_init(void)
129 {
130  electrical.vsupply = 0.f;
131  electrical.vboard = 0.f;
132  electrical.current = 0.f;
133  electrical.charge = 0.f;
134  electrical.energy = 0.f;
135  electrical.avg_power = 0;
136  electrical.avg_cnt = 0;
137 
138  electrical.bat_low = false;
139  electrical.bat_critical = false;
140 
141 #if defined ADC_CHANNEL_VSUPPLY
142  adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
143 #endif
144 
145 #if defined ADC_CHANNEL_VBOARD
146  adc_buf_channel(ADC_CHANNEL_VBOARD, &electrical_priv.vboard_adc_buf, DEFAULT_AV_NB_SAMPLE);
147 #endif
148 
149  /* measure current if available, otherwise estimate it */
150 #if defined ADC_CHANNEL_CURRENT && !defined SITL
151  adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);
152 
153 #if defined ADC_CHANNEL_CURRENT2 && !defined SITL
154  adc_buf_channel(ADC_CHANNEL_CURRENT2, &electrical_priv.current2_adc_buf, DEFAULT_AV_NB_SAMPLE);
155 #endif
156 #elif defined MILLIAMP_AT_FULL_THROTTLE
157  electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
158 #endif
159 
160  /* Register preflight checks */
161 #if PREFLIGHT_CHECKS
162  preflight_check_register(&electrical_pfc, electrical_preflight);
163 #endif
164 }
165 
167 {
168  static uint32_t bat_low_counter = 0;
169  static uint32_t bat_critical_counter = 0;
170  static bool vsupply_check_started = false;
171 
172 #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) && !USE_BATTERY_MONITOR
173  electrical.vsupply = VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum /
174  electrical_priv.vsupply_adc_buf.av_nb_sample));
175 #endif
176 
177 #if defined(ADC_CHANNEL_VBOARD) && !defined(SITL)
178  electrical.vboard = VBoardOfAdc((electrical_priv.vboard_adc_buf.sum /
179  electrical_priv.vboard_adc_buf.av_nb_sample));
180 #endif
181 
182 #ifdef ADC_CHANNEL_CURRENT
183 #ifndef SITL
184  int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample;
185  electrical.current = MilliAmpereOfAdc(current_adc) / 1000.f;
186 
187 #ifdef ADC_CHANNEL_CURRENT2
188  current_adc = electrical_priv.current2_adc_buf.sum / electrical_priv.current2_adc_buf.av_nb_sample;
189  electrical.current += MilliAmpereOfAdc2(current_adc) / 1000.f;
190 #endif
191 #endif
192 #elif defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_CURRENT_ESTIMATION
193  /*
194  * Superellipse: abs(x/a)^n + abs(y/b)^n = 1
195  * with a = 1
196  * b = mA at full throttle
197  * n = 1.2 This defines nonlinearity (1 = linear)
198  * x = throttle
199  * y = current
200  *
201  * define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
202  */
203  static float full_current = (float)MILLIAMP_AT_FULL_THROTTLE / 1000.f;
204  static float idle_current = (float)MILLIAMP_AT_IDLE_THROTTLE / 1000.f;
205 
206  float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ);
207 
208  /* Boundary check for x to prevent math errors due to negative numbers in
209  * pow() */
210  Bound(x, 0.f, 1.f);
211 
212  /* electrical.current y = ( b^n - (b* x/a)^n )^1/n
213  * a=1, n = electrical_priv.nonlin_factor
214  */
215 #ifndef FBW
217  // Assume no current when throttle killed (motors off)
218  electrical.current = 0;
219  } else {
220 #endif
221  electrical.current = full_current -
222  powf((powf(full_current - idle_current, electrical_priv.nonlin_factor) -
223  powf(((full_current - idle_current) * x), electrical_priv.nonlin_factor)),
224  (1.f / electrical_priv.nonlin_factor));
225 #ifndef FBW
226  }
227 #endif
228 #endif /* ADC_CHANNEL_CURRENT */
229 
230  static const float period_to_hour = 1 / 3600.f / ELECTRICAL_PERIODIC_FREQ;
231  float consumed_since_last = electrical.current * period_to_hour;
232 
233  electrical.charge += consumed_since_last;
234  electrical.energy += consumed_since_last * electrical.vsupply;
235 
236  /*if valid voltage is seen then start checking. Set min level to 0 to always start*/
238  vsupply_check_started = true;
239  }
240 
241  if (vsupply_check_started) {
243  if (bat_low_counter > 0) {
244  bat_low_counter--;
245  }
246  if (bat_low_counter == 0) {
247  electrical.bat_low = true;
248  }
249  } else {
250  // reset battery low status and counter
251  bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
252  electrical.bat_low = false;
253  }
254 
256  if (bat_critical_counter > 0) {
257  bat_critical_counter--;
258  }
259  if (bat_critical_counter == 0) {
260  electrical.bat_critical = true;
261  }
262  } else {
263  // reset battery critical status and counter
264  bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
265  electrical.bat_critical = false;
266  }
267  }
268 
269  float power = electrical.vsupply * electrical.current;
270  electrical.avg_power += power;
272 }
273 
274 void electrical_avg_reset(float var __attribute__((unused)))
275 {
276  electrical.avg_power = 0;
277  electrical.avg_cnt = 0;
278 }
arch independent ADC (Analog to Digital Converter) API
#define DEFAULT_AV_NB_SAMPLE
Definition: adc.h:41
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:53
#define ADC_CHANNEL_VSUPPLY
Definition: board.h:197
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:308
Core autopilot interface common to all firmwares.
void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
Link between ChibiOS ADC drivers and Paparazzi adc_buffers.
Definition: adc_arch.c:312
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
Hardware independent code for commands handling.
#define ADC_CHANNEL_CURRENT
#define ADC_CHANNEL_VBOARD
Definition: board.h:50
void electrical_avg_reset(float var)
Definition: electrical.c:274
#define BAT_CHECKER_DELAY
Definition: electrical.c:53
#define VoltageOfAdc(adc)
Definition: electrical.c:70
#define MilliAmpereOfAdc(adc)
Definition: electrical.c:76
#define VBoardOfAdc(adc)
Definition: electrical.c:73
struct Electrical electrical
Definition: electrical.c:92
#define TAKEOFF_BAT_LEVEL
Definition: electrical.c:61
#define CURRENT_ESTIMATION_NONLINEARITY
Definition: electrical.c:80
void electrical_init(void)
Definition: electrical.c:128
#define MIN_BAT_LEVEL
Definition: electrical.c:57
void electrical_periodic(void)
Definition: electrical.c:166
Interface for electrical status: supply voltage, current, battery status, etc.
uint32_t avg_power
average power sum
Definition: electrical.h:53
#define LOW_BAT_LEVEL
low battery level in Volts (for 3S LiPo)
Definition: electrical.h:36
float energy
consumed energy in Wh
Definition: electrical.h:49
float current
current in A
Definition: electrical.h:47
uint32_t avg_cnt
average power counter
Definition: electrical.h:54
#define CRITIC_BAT_LEVEL
critical battery level in Volts (for 3S LiPo)
Definition: electrical.h:41
bool bat_low
battery low status
Definition: electrical.h:50
float charge
consumed electric charge in Ah
Definition: electrical.h:48
float vboard
board voltage in V
Definition: electrical.h:46
float vsupply
supply voltage in V
Definition: electrical.h:45
bool bat_critical
battery critical status
Definition: electrical.h:51
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define MAX_PPRZ
Definition: paparazzi.h:8
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78