breezy_board.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #pragma GCC diagnostic push
33 #pragma GCC diagnostic ignored "-Wold-style-cast"
34 
35 extern "C"
36 {
37 #include "flash.h"
38 
39 #include <breezystm32.h>
40  extern void SetSysClock(bool overclock);
41 }
42 
43 #include "breezy_board.h"
44 
45 namespace rosflight_firmware
46 {
48 
50 {
51  // Configure clock, this figures out HSE for hardware autodetect
52  SetSysClock(0);
53  systemInit();
54  _board_revision = 2;
55 }
56 
57 void BreezyBoard::board_reset(bool bootloader)
58 {
59  systemReset(bootloader);
60 }
61 
62 // clock
63 
65 {
66  return millis();
67 }
68 
70 {
71  return micros();
72 }
73 
74 void BreezyBoard::clock_delay(uint32_t milliseconds)
75 {
76  delay(milliseconds);
77 }
78 
79 // serial
80 
81 void BreezyBoard::serial_init(uint32_t baud_rate, uint32_t dev)
82 {
83  (void)dev;
84  Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX);
85 }
86 
87 void BreezyBoard::serial_write(const uint8_t *src, size_t len)
88 {
89  for (size_t i = 0; i < len; i++)
90  {
91  serialWrite(Serial1, src[i]);
92  }
93 }
94 
96 {
98 }
99 
101 {
102  return serialRead(Serial1);
103 }
104 
106 {
107  return;
108 }
109 
110 // sensors
111 
113 {
114  // Initialize I2c
115  i2cInit(I2CDEV_2);
116 
117  while (millis() < 50)
118  ;
119 
120  i2cWrite(0, 0, 0);
121  if (bmp280_init())
123  else if (ms5611_init())
125 
126  hmc5883lInit();
127  mb1242_init();
128  ms4525_init();
129 
130  // IMU
131  uint16_t acc1G;
132  mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision);
133  _accel_scale = 9.80665f / acc1G;
134 }
135 
137 {
138  return i2cGetErrorCounter();
139 }
140 
142 {
143  return mpu6050_new_data();
144 }
145 
146 bool BreezyBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us)
147 {
148  volatile int16_t gyro_raw[3], accel_raw[3];
149  volatile int16_t raw_temp;
150  mpu6050_async_read_all(accel_raw, &raw_temp, gyro_raw, time_us);
151 
152  accel[0] = accel_raw[0] * _accel_scale;
153  accel[1] = -accel_raw[1] * _accel_scale;
154  accel[2] = -accel_raw[2] * _accel_scale;
155 
156  gyro[0] = gyro_raw[0] * _gyro_scale;
157  gyro[1] = -gyro_raw[1] * _gyro_scale;
158  gyro[2] = -gyro_raw[2] * _gyro_scale;
159 
160  (*temperature) = (float)raw_temp / 340.0f + 36.53f;
161 
162  if (accel[0] == 0 && accel[1] == 0 && accel[2] == 0)
163  {
164  return false;
165  }
166  else
167  return true;
168 }
169 
171 {
172  // If the IMU is not responding, then we need to change where we look for the interrupt
173  _board_revision = (_board_revision < 4) ? 5 : 2;
174  sensors_init();
175 }
176 
177 void BreezyBoard::mag_read(float mag[3])
178 {
179  // Convert to NED
180  hmc5883l_async_read(mag);
181 }
182 
184 {
185  return hmc5883l_present();
186 }
187 
189 {
191 }
192 
194 {
195  if (baro_type == BARO_BMP280)
197  else if (baro_type == BARO_MS5611)
199  else
200  {
203  }
204 }
205 
207 {
208  if (baro_type == BARO_BMP280)
209  {
211  bmp280_async_read(pressure, temperature);
212  }
213  else if (baro_type == BARO_MS5611)
214  {
216  ms5611_async_read(pressure, temperature);
217  }
218 }
219 
221 {
222  if (baro_type == BARO_BMP280)
223  return bmp280_present();
224  else if (baro_type == BARO_MS5611)
225  return ms5611_present();
226  else
227  {
228  if (bmp280_present())
229  {
231  return true;
232  }
233  else if (ms5611_present())
234  {
236  return true;
237  }
238  }
239  return false;
240 }
241 
243 {
244  return ms4525_present();
245 }
246 
248 {
249  return ms4525_async_update();
250 }
251 
252 void BreezyBoard::diff_pressure_read(float *diff_pressure, float *temperature)
253 {
255  ms4525_async_read(diff_pressure, temperature);
256 }
257 
259 {
262 
263  // We don't need to actively update the pwm sonar
264 }
265 
267 {
268  if (sonar_type == SONAR_I2C)
269  return mb1242_present();
270  else if (sonar_type == SONAR_PWM)
271  return sonarPresent();
272  else
273  {
274  if (mb1242_present())
275  {
277  return true;
278  }
279  else if (sonarPresent())
280  {
282  return true;
283  }
284  }
285  return false;
286 }
287 
289 {
290  if (sonar_type == SONAR_I2C)
291  {
293  return mb1242_async_read();
294  }
295  else if (sonar_type == SONAR_PWM)
296  return sonarRead(6);
297  else
298  return 0.0f;
299 }
300 
302 {
303  return i2cGetErrorCounter();
304 }
305 
307 {
308  return false;
309 }
311 {
312  return 0;
313 }
315 {
316  (void)multiplier;
317 }
318 
320 {
321  return false;
322 }
324 {
325  return 0;
326 }
328 {
329  (void)multiplier;
330 }
331 // PWM
332 
334 {
335  (void)rc_type; // TODO SBUS is not supported on F1
336  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
337 }
338 
339 void BreezyBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)
340 {
341  pwm_refresh_rate_ = refresh_rate;
342  pwm_idle_pwm_ = idle_pwm;
343  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
344 }
345 
347 {
348  pwm_refresh_rate_ = 50;
349  pwm_idle_pwm_ = 0;
350  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
351 }
352 
353 float BreezyBoard::rc_read(uint8_t channel)
354 {
355  return (float)(pwmRead(channel) - 1000) / 1000.0;
356 }
357 
358 void BreezyBoard::pwm_write(uint8_t channel, float value)
359 {
360  pwmWriteMotor(channel, static_cast<uint16_t>(value * 1000) + 1000);
361 }
362 
364 {
365  return ((millis() - pwmLastUpdate()) > 40);
366 }
367 
368 // non-volatile memory
369 
371 {
372  initEEPROM();
373 }
374 
375 bool BreezyBoard::memory_read(void *dest, size_t len)
376 {
377  return readEEPROM(dest, len);
378 }
379 
380 bool BreezyBoard::memory_write(const void *src, size_t len)
381 {
382  return writeEEPROM(src, len);
383 }
384 
385 // GNSS is not supported on breezy boards
387 {
388  return {};
389 }
390 
391 // GNSS is not supported on breezy boards
393 {
394  return {};
395 }
396 
397 // GNSS is not supported on breezy boards
399 {
400  return false;
401 }
402 
403 // LED
404 
406 {
407  LED0_ON;
408 }
409 
411 {
412  LED0_OFF;
413 }
414 
416 {
417  LED0_TOGGLE;
418 }
419 
421 {
422  LED1_ON;
423 }
424 
426 {
427  LED1_OFF;
428 }
429 
431 {
432  LED1_TOGGLE;
433 }
434 
435 } // namespace rosflight_firmware
436 
437 #pragma GCC diagnostic pop
void pwm_write(uint8_t channel, float value) override
uint16_t i2cGetErrorCounter(void)
Definition: drv_i2c.c:467
void systemInit(void)
Definition: system.c:55
bool hmc5883lInit()
Definition: drv_hmc5883l.c:83
uint16_t serial_bytes_available() override
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
bool ms4525_init()
Definition: drv_ms4525.c:40
#define LED1_ON
Definition: drv_system.h:53
void pwmInit(bool useCPPM, bool usePwmFilter, bool fastPWM, uint32_t motorPwmRate, uint16_t idlePulseUsec)
Definition: drv_pwm.c:353
#define LED0_TOGGLE
Definition: drv_system.h:41
void i2cInit(I2CDevice index)
Definition: drv_i2c.c:420
void serial_init(uint32_t baud_rate, uint32_t dev) override
#define LED1_OFF
Definition: drv_system.h:52
volatile uint32_t millis(void)
Definition: system.c:50
float pressure
Definition: ms4525.c:41
void battery_current_set_multiplier(double multiplier) override
bool hmc5883l_present()
Definition: drv_hmc5883l.c:111
float sonarRead(uint8_t channel)
Definition: drv_pwm.c:427
float temperature
Definition: ms4525.c:41
void mb1242_async_update()
Definition: drv_mb1242.c:76
bool memory_read(void *dest, size_t len) override
bool ms5611_init()
Definition: drv_ms5611.c:151
#define LED0_ON
Definition: drv_system.h:43
void mpu6050_async_read_all(volatile int16_t *accData, volatile int16_t *tempData, volatile int16_t *gyroData, volatile uint64_t *timeData)
Definition: drv_mpu6050.c:316
void SetSysClock(bool overclock)
volatile uint64_t micros(void)
Definition: system.c:44
void mpu6050_init(bool enableInterrupt, uint16_t *acc1G, float *gyroScale, int boardVersion)
Definition: drv_mpu6050.c:175
bool mb1242_init()
Definition: drv_mb1242.c:57
void ms4525_async_read(float *differential_pressure, float *temp)
Definition: drv_ms4525.c:80
bool mb1242_present()
Definition: drv_mb1242.c:69
uint32_t pwmLastUpdate()
Definition: drv_pwm.c:186
float battery_current_read() const override
void systemReset(void)
void hmc5883l_async_read(float *mag_data)
Definition: drv_hmc5883l.c:138
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
Definition: drv_i2c.c:152
bool battery_current_present() const override
void diff_pressure_read(float *diff_pressure, float *temperature) override
#define USART1
Definition: stm32f4xx.h:2080
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
bool ms4525_present()
Definition: drv_ms4525.c:62
bool sonarPresent()
Definition: drv_pwm.c:432
bool mpu6050_new_data()
Definition: drv_mpu6050.c:328
GNSSFull gnss_full_read() override
void bmp280_async_read(float *pres, float *temp)
Definition: drv_bmp280.c:180
void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override
void initEEPROM(void)
Initialize Flash.
Definition: flash.c:37
bool memory_write(const void *src, size_t len) override
void board_reset(bool bootloader) override
bool readEEPROM(void *dest, size_t len)
Read data from Flash.
Definition: flash.c:54
uint16_t pwmRead(uint8_t channel)
Definition: drv_pwm.c:409
float rc_read(uint8_t channel) override
bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override
void clock_delay(uint32_t milliseconds) override
void imu_not_responding_error() override
void battery_voltage_set_multiplier(double multiplier) override
void baro_read(float *pressure, float *temperature) override
uint32_t clock_millis() override
bool writeEEPROM(const void *src, size_t len)
Write data to Flash.
Definition: flash.c:60
void serialWrite(serialPort_t *instance, uint8_t ch)
Definition: drv_serial.c:40
bool bmp280_init()
Definition: drv_bmp280.c:69
serialPort_t * uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode)
Definition: drv_uart.c:80
uint16_t num_sensor_errors() override
#define LED1_TOGGLE
Definition: drv_system.h:51
bool bmp280_present()
Definition: drv_bmp280.c:64
void hmc5883l_request_async_update()
Definition: drv_hmc5883l.c:118
bool battery_voltage_present() const override
void bmp280_async_update()
Definition: drv_bmp280.c:163
#define NULL
Definition: usbd_def.h:50
void ms5611_async_update()
Definition: drv_ms5611.c:405
bool ms5611_present()
Definition: drv_ms5611.c:203
uint64_t clock_micros() override
float mb1242_async_read()
Definition: drv_mb1242.c:99
void serial_write(const uint8_t *src, size_t len) override
void mag_read(float mag[3]) override
void ms4525_async_update()
Definition: drv_ms4525.c:53
uint8_t serialRead(serialPort_t *instance)
Definition: drv_serial.c:50
float battery_voltage_read() const override
#define LED0_OFF
Definition: drv_system.h:42
void pwmWriteMotor(uint8_t index, uint16_t value)
Definition: drv_pwm.c:403
void delay(uint32_t ms)
Definition: system.c:101
void ms5611_async_read(float *pressure, float *temperature)
Definition: drv_ms5611.c:452
uint8_t serialTotalBytesWaiting(serialPort_t *instance)
Definition: drv_serial.c:45
void rc_init(rc_type_t rc_type) override


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:07:46