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 
38 #include <breezystm32.h>
39 #include "flash.h"
40  extern void SetSysClock(bool overclock);
41 
42 }
43 
44 
45 #include "breezy_board.h"
46 
47 namespace rosflight_firmware
48 {
49 
51 
53 {
54  // Configure clock, this figures out HSE for hardware autodetect
55  SetSysClock(0);
56  systemInit();
57  _board_revision = 2;
58 }
59 
60 void BreezyBoard::board_reset(bool bootloader)
61 {
62  systemReset(bootloader);
63 }
64 
65 // clock
66 
68 {
69  return millis();
70 }
71 
73 {
74  return micros();
75 }
76 
77 void BreezyBoard::clock_delay(uint32_t milliseconds)
78 {
79  delay(milliseconds);
80 }
81 
82 // serial
83 
84 void BreezyBoard::serial_init(uint32_t baud_rate, uint32_t dev)
85 {
86  (void)dev;
87  Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX);
88 }
89 
90 void BreezyBoard::serial_write(const uint8_t *src, size_t len)
91 {
92  for (size_t i = 0; i < len; i++)
93  {
94  serialWrite(Serial1, src[i]);
95  }
96 }
97 
99 {
101 }
102 
104 {
105  return serialRead(Serial1);
106 }
107 
109 {
110  return;
111 }
112 
113 // sensors
114 
116 {
117  // Initialize I2c
118  i2cInit(I2CDEV_2);
119 
120  while (millis() < 50);
121 
122  i2cWrite(0,0,0);
123  if (bmp280_init())
125  else if (ms5611_init())
127 
128  hmc5883lInit();
129  mb1242_init();
130  ms4525_init();
131 
132 
133  // IMU
134  uint16_t acc1G;
135  mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision);
136  _accel_scale = 9.80665f/acc1G;
137 }
138 
140 {
141  return i2cGetErrorCounter();
142 }
143 
145 {
146  return mpu6050_new_data();
147 }
148 
149 bool BreezyBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us)
150 {
151  volatile int16_t gyro_raw[3], accel_raw[3];
152  volatile int16_t raw_temp;
153  mpu6050_async_read_all(accel_raw, &raw_temp, gyro_raw, time_us);
154 
155  accel[0] = accel_raw[0] * _accel_scale;
156  accel[1] = -accel_raw[1] * _accel_scale;
157  accel[2] = -accel_raw[2] * _accel_scale;
158 
159  gyro[0] = gyro_raw[0] * _gyro_scale;
160  gyro[1] = -gyro_raw[1] * _gyro_scale;
161  gyro[2] = -gyro_raw[2] * _gyro_scale;
162 
163  (*temperature) = (float)raw_temp/340.0f + 36.53f;
164 
165  if (accel[0] == 0 && accel[1] == 0 && accel[2] == 0)
166  {
167  return false;
168  }
169  else return true;
170 }
171 
173 {
174  // If the IMU is not responding, then we need to change where we look for the interrupt
175  _board_revision = (_board_revision < 4) ? 5 : 2;
176  sensors_init();
177 }
178 
179 void BreezyBoard::mag_read(float mag[3])
180 {
181  // Convert to NED
182  hmc5883l_async_read(mag);
183 }
184 
186 {
187  return hmc5883l_present();
188 }
189 
191 {
193 }
194 
196 {
197  if (baro_type == BARO_BMP280)
199  else if (baro_type == BARO_MS5611)
201  else
202  {
205  }
206 }
207 
208 
209 
211 {
212  if (baro_type == BARO_BMP280)
213  {
215  bmp280_async_read(pressure, temperature);
216  }
217  else if (baro_type == BARO_MS5611)
218  {
220  ms5611_async_read(pressure, temperature);
221  }
222 }
223 
225 {
226  if (baro_type == BARO_BMP280)
227  return bmp280_present();
228  else if (baro_type == BARO_MS5611)
229  return ms5611_present();
230  else
231  {
232  if (bmp280_present())
233  {
235  return true;
236  }
237  else if (ms5611_present())
238  {
240  return true;
241  }
242  }
243  return false;
244 }
245 
247 {
248  return ms4525_present();
249 }
250 
252 {
253  return ms4525_async_update();
254 }
255 
256 void BreezyBoard::diff_pressure_read(float *diff_pressure, float *temperature)
257 {
259  ms4525_async_read(diff_pressure, temperature);
260 }
261 
263 {
266 
267  // We don't need to actively update the pwm sonar
268 }
269 
271 {
272  if (sonar_type == SONAR_I2C)
273  return mb1242_present();
274  else if (sonar_type == SONAR_PWM)
275  return sonarPresent();
276  else
277  {
278  if (mb1242_present())
279  {
281  return true;
282  }
283  else if (sonarPresent())
284  {
286  return true;
287  }
288  }
289  return false;
290 }
291 
293 {
294  if (sonar_type == SONAR_I2C)
295  {
297  return mb1242_async_read();
298  }
299  else if (sonar_type == SONAR_PWM)
300  return sonarRead(6);
301  else
302  return 0.0f;
303 }
304 
306 {
307  return i2cGetErrorCounter();
308 }
309 
310 // PWM
311 
313 {
314  (void) rc_type; // TODO SBUS is not supported on F1
315  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
316 }
317 
318 void BreezyBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)
319 {
320  pwm_refresh_rate_ = refresh_rate;
321  pwm_idle_pwm_ = idle_pwm;
322  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
323 }
324 
326 {
327  pwm_refresh_rate_ = 50;
328  pwm_idle_pwm_ = 0;
329  pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_);
330 }
331 
332 float BreezyBoard::rc_read(uint8_t channel)
333 {
334  return (float)(pwmRead(channel) - 1000)/1000.0;
335 }
336 
337 void BreezyBoard::pwm_write(uint8_t channel, float value)
338 {
339  pwmWriteMotor(channel, static_cast<uint16_t>(value * 1000) + 1000);
340 }
341 
343 {
344  return ((millis() - pwmLastUpdate()) > 40);
345 }
346 
347 // non-volatile memory
348 
350 {
351  initEEPROM();
352 }
353 
354 bool BreezyBoard::memory_read(void *dest, size_t len)
355 {
356  return readEEPROM(dest, len);
357 }
358 
359 bool BreezyBoard::memory_write(const void *src, size_t len)
360 {
361  return writeEEPROM(src, len);
362 }
363 
364 //GNSS is not supported on breezy boards
366 {
367  return {};
368 }
369 
370 //GNSS is not supported on breezy boards
372 {
373  return {};
374 }
375 
376 //GNSS is not supported on breezy boards
378 {
379  return false;
380 }
381 
382 // LED
383 
385 {
386  LED0_ON;
387 }
388 
390 {
391  LED0_OFF;
392 }
393 
395 {
396  LED0_TOGGLE;
397 }
398 
400 {
401  LED1_ON;
402 }
403 
405 {
406  LED1_OFF;
407 }
408 
410 {
411  LED1_TOGGLE;
412 }
413 
415 {
416  return false;
417 }
418 
420 {
421 #pragma GCC diagnostic push
422 #pragma GCC diagnostic ignored "-Wmissing-field-initializers"
423  BackupData blank_data = {0};
424 #pragma GCC diagnostic pop
425  return blank_data;
426 }
427 
428 }
429 
430 #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
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
GNSSRaw gnss_raw_read() override
#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
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
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
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
BackupData get_backup_data() 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 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
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
#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:98
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 Oct 24 2019 03:17:18