airbourne_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 #include "airbourne_board.h"
33 
35 {
37 
39 {
40  systemInit();
43 
50 
52 
53  current_serial_ = &vcp_; // uncomment this to switch to VCP as the main output
54 }
55 
56 void AirbourneBoard::board_reset(bool bootloader)
57 {
58  (void)bootloader;
60 }
61 
62 // clock
64 {
65  return millis();
66 }
67 
69 {
70  return micros();
71 }
72 
73 void AirbourneBoard::clock_delay(uint32_t milliseconds)
74 {
75  delay(milliseconds);
76 }
77 
78 // serial
79 void AirbourneBoard::serial_init(uint32_t baud_rate, uint32_t dev)
80 {
81  vcp_.init();
82  switch (dev)
83  {
85  uart3_.init(&uart_config[UART3], baud_rate);
88  break;
89  default:
92  }
93 }
94 
95 void AirbourneBoard::serial_write(const uint8_t *src, size_t len)
96 {
97  current_serial_->write(src, len);
98 }
99 
101 {
103  {
105  }
106  else
107  {
108  switch (secondary_serial_device_)
109  {
110  case SERIAL_DEVICE_UART3:
112  break;
113  default:
114  // no secondary serial device
115  break;
116  }
117  }
118 
120 }
121 
123 {
124  return current_serial_->read_byte();
125 }
126 
128 {
130 }
131 
132 // sensors
134 {
135  while (millis() < 50)
136  {
137  } // wait for sensors to boot up
138  imu_.init(&spi1_);
139 
140  baro_.init(&int_i2c_);
141  mag_.init(&int_i2c_);
142  sonar_.init(&ext_i2c_);
144  gnss_.init(&uart1_);
147 }
148 
150 {
151  return ext_i2c_.num_errors();
152 }
153 
155 {
156  return imu_.new_data();
157 }
158 
159 bool AirbourneBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us)
160 {
161  float read_accel[3], read_gyro[3];
162  imu_.read(read_accel, read_gyro, temperature, time_us);
163 
164  accel[0] = -read_accel[1];
165  accel[1] = -read_accel[0];
166  accel[2] = -read_accel[2];
167 
168  gyro[0] = -read_gyro[1];
169  gyro[1] = -read_gyro[0];
170  gyro[2] = -read_gyro[2];
171 
172  return true;
173 }
174 
176 {
177  sensors_init();
178 }
179 
181 {
182  mag_.update();
183  return mag_.present();
184 }
185 
187 {
188  mag_.update();
189 }
190 
191 void AirbourneBoard::mag_read(float mag[3])
192 {
193  mag_.update();
194  mag_.read(mag);
195 }
197 {
198  baro_.update();
199  return baro_.present();
200 }
201 
203 {
204  baro_.update();
205 }
206 
208 {
209  baro_.update();
210  baro_.read(pressure, temperature);
211 }
212 
214 {
215  return airspeed_.present();
216 }
217 
219 {
220  airspeed_.update();
221 }
222 
223 void AirbourneBoard::diff_pressure_read(float *diff_pressure, float *temperature)
224 {
225  (void)diff_pressure;
226  (void)temperature;
227  airspeed_.update();
228  airspeed_.read(diff_pressure, temperature);
229 }
230 
232 {
233  return sonar_.present();
234 }
235 
237 {
238  sonar_.update();
239 }
240 
242 {
243  return sonar_.read();
244 }
245 
247 {
249  return gnss_.present();
250 }
253 {
254  return this->gnss_.new_data();
255 }
256 // This method translates the UBLOX driver interface into the ROSFlight interface
257 // If not gnss_has_new_data(), then this may return 0's for ECEF position data,
258 // ECEF velocity data, or both
260 {
261  UBLOX::GNSSPVT gnss_pvt = gnss_.read();
264  uint64_t timestamp = gnss_.get_last_pvt_timestamp();
265  GNSSData gnss = {};
266  gnss.time_of_week = gnss_pvt.time_of_week;
267  bool has_fix = (gnss_pvt.fix_type == UBLOX::FIX_TYPE_3D);
269  gnss.time = gnss_pvt.time;
270  gnss.nanos = gnss_pvt.nanos;
271  gnss.lat = gnss_pvt.lat;
272  gnss.lon = gnss_pvt.lon;
273  gnss.height = gnss_pvt.height;
274  gnss.vel_n = gnss_pvt.vel_n;
275  gnss.vel_e = gnss_pvt.vel_e;
276  gnss.vel_d = gnss_pvt.vel_d;
277  gnss.h_acc = gnss_pvt.h_acc;
278  gnss.v_acc = gnss_pvt.v_acc;
279  // Does not include ECEF position data if the timestamp doesn't match
280  // See UBLOX::new_data() for reasoning
281  if (gnss.time_of_week == pos_ecef.time_of_week)
282  {
283  gnss.ecef.x = pos_ecef.x;
284  gnss.ecef.y = pos_ecef.y;
285  gnss.ecef.z = pos_ecef.z;
286  gnss.ecef.p_acc = pos_ecef.p_acc;
287  }
288  // Does not include ECEF position data if the timestamp doesn't match
289  // See UBLOX::new_data() for reasoning
290  if (gnss.time_of_week == vel_ecef.time_of_week)
291  {
292  gnss.ecef.vx = vel_ecef.vx;
293  gnss.ecef.vy = vel_ecef.vy;
294  gnss.ecef.vz = vel_ecef.vz;
295  gnss.ecef.s_acc = vel_ecef.s_acc;
296  }
297  gnss.rosflight_timestamp = timestamp;
298  return gnss;
299 }
301 {
302  UBLOX::NAV_PVT_t pvt = gnss_.read_full();
303  GNSSFull full = {};
304  full.time_of_week = pvt.iTOW;
305  full.year = pvt.time.year;
306  full.month = pvt.time.month;
307  full.day = pvt.time.day;
308  full.hour = pvt.time.hour;
309  full.min = pvt.time.min;
310  full.sec = pvt.time.sec;
311  full.valid = pvt.time.valid;
312  full.t_acc = pvt.time.tAcc;
313  full.nano = pvt.time.nano;
314  full.fix_type = pvt.fixType;
315  full.num_sat = pvt.numSV;
316  full.lon = pvt.lon;
317  full.lat = pvt.lat;
318  full.height = pvt.height;
319  full.height_msl = pvt.hMSL;
320  full.h_acc = pvt.hAcc;
321  full.v_acc = pvt.vAcc;
322  full.vel_n = pvt.velN;
323  full.vel_e = pvt.velE;
324  full.vel_d = pvt.velD;
325  full.g_speed = pvt.gSpeed;
326  full.head_mot = pvt.headMot;
327  full.s_acc = pvt.sAcc;
328  full.head_acc = pvt.headAcc;
329  full.p_dop = pvt.pDOP;
331  return full;
332 }
333 
335 {
336  return this->battery_monitor_.has_voltage_sense();
337 }
338 
340 {
341  return static_cast<float>(this->battery_monitor_.read_voltage());
342 }
343 
345 {
346  this->battery_monitor_.set_voltage_multiplier(multiplier);
347 }
348 
350 {
351  return this->battery_monitor_.has_current_sense();
352 }
353 
355 {
356  return static_cast<float>(this->battery_monitor_.read_current());
357 }
358 
360 {
361  this->battery_monitor_.set_current_multiplier(multiplier);
362 }
363 
364 // PWM
366 {
367  switch (rc_type)
368  {
369  case RC_TYPE_SBUS:
373  rc_ = &rc_sbus_;
374  break;
375  case RC_TYPE_PPM:
376  default:
378  rc_ = &rc_ppm_;
379  break;
380  }
381 }
382 
383 float AirbourneBoard::rc_read(uint8_t channel)
384 {
385  return rc_->read(channel);
386 }
387 
388 void AirbourneBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)
389 {
390  for (int i = 0; i < PWM_NUM_OUTPUTS; i++)
391  {
392  esc_out_[i].init(&pwm_config[i], refresh_rate, 2000, 1000);
393  esc_out_[i].writeUs(idle_pwm);
394  }
395 }
396 
398 {
399  for (int i = 0; i < PWM_NUM_OUTPUTS; i++)
400  {
401  esc_out_[i].disable();
402  }
403 }
404 
405 void AirbourneBoard::pwm_write(uint8_t channel, float value)
406 {
407  if (channel < PWM_NUM_OUTPUTS)
408  {
409  esc_out_[channel].write(value);
410  }
411 }
412 
414 {
415  return rc_->lost();
416 }
417 
418 // non-volatile memory
420 {
421  return flash_.init(&spi3_);
422 }
423 
424 bool AirbourneBoard::memory_read(void *data, size_t len)
425 {
426  return flash_.read_config(reinterpret_cast<uint8_t *>(data), len);
427 }
428 
429 bool AirbourneBoard::memory_write(const void *data, size_t len)
430 {
431  return flash_.write_config(reinterpret_cast<const uint8_t *>(data), len);
432 }
433 
434 // LED
436 {
437  led1_.on();
438 }
439 
441 {
442  led1_.off();
443 }
444 
446 {
447  led1_.toggle();
448 }
449 
451 {
452  led2_.on();
453 }
454 
456 {
457  led2_.off();
458 }
459 
461 {
462  led2_.toggle();
463 }
464 
465 // Backup memory
467 {
469 }
470 
471 bool AirbourneBoard::backup_memory_read(void *dest, size_t len)
472 {
473  backup_sram_read(dest, len);
474  return true;
475 }
476 
477 void AirbourneBoard::backup_memory_write(const void *src, size_t len)
478 {
479  backup_sram_write(src, len);
480 }
481 
483 {
484  backup_sram_clear(len);
485 }
486 
487 } // namespace rosflight_firmware
uint32_t time_of_week
Definition: ublox.h:434
void systemInit(void)
Definition: system.c:55
bool present()
Definition: ms4525.cpp:57
bool present()
Definition: mb1242.cpp:73
void init(const uart_hardware_struct_t *conf, uint32_t baudrate_, uart_mode_t mode=MODE_8N1)
Definition: uart.cpp:39
int32_t vel_e
Definition: ublox.h:415
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
uint64_t get_last_pvt_timestamp()
Definition: ublox.h:458
void backup_memory_write(const void *src, size_t len) override
void toggle(void)
Definition: gpio.cpp:52
#define UART1
Definition: revo_f4.h:61
void init(const pwm_hardware_struct_t *conf)
Definition: rc_ppm.cpp:36
void baro_read(float *pressure, float *temperature) override
volatile uint32_t millis(void)
Definition: system.c:50
float pressure
Definition: ms4525.c:41
bool connected()
Definition: vcp.cpp:108
uint32_t time_of_week
Definition: ublox.h:407
float temperature
Definition: ms4525.c:41
virtual bool lost()=0
bool memory_write(const void *src, size_t len) override
#define MPU6000_SPI
Definition: revo_f4.h:106
void init(GPIO_TypeDef *gpio_port, uint16_t pin)
Definition: led.cpp:34
bool has_current_sense() const
Checks if current sense is available. As there isn&#39;t hardware to detect this, this simply checks if t...
const BatteryMonitorHardwareStruct battery_monitor_config
Definition: revo_f4.h:290
AnalogDigitalConverter battery_adc_
int32_t vy
Definition: ublox.h:436
void backup_sram_init()
Definition: backup_sram.cpp:8
#define LED2_PIN
Definition: revo_f4.h:129
virtual bool flush()=0
void diff_pressure_read(float *diff_pressure, float *temperature) override
int32_t nanos
Definition: ublox.h:410
bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override
void set_voltage_multiplier(double multiplier)
Sets the voltage multiplier for the battery monitor.
void init(const i2c_hardware_struct_t *c)
Definition: i2c.cpp:52
void init()
Definition: vcp.cpp:24
virtual uint8_t read_byte()=0
bool has_voltage_sense() const
Checks if voltage sense is available. As there isn&#39;t hardware to detect this, this simply checks if t...
const struct ADCHardwareStruct * adc
Definition: system.h:140
float read_current() const
Read the current battery current.
#define LED2_GPIO
Definition: revo_f4.h:128
void clock_delay(uint32_t milliseconds) override
volatile uint64_t micros(void)
Definition: system.c:44
uint32_t s_acc
Definition: ublox.h:438
void backup_sram_read(void *dst, size_t len)
Definition: backup_sram.cpp:29
bool present()
Definition: hmc5883l.cpp:69
void init(UART *uart)
Definition: ublox.cpp:16
#define LED1_GPIO
Definition: revo_f4.h:126
FixType fix_type
Definition: ublox.h:408
void init(GPIO_TypeDef *BasePort, uint16_t pin, gpio_mode_t mode)
Definition: gpio.cpp:34
uint16_t num_errors()
Definition: i2c.h:145
#define PWM_NUM_OUTPUTS
Definition: revo_f4.h:133
bool init(I2C *_i2c)
Definition: ms5611.cpp:39
void backup_memory_clear(size_t len) override
#define FLASH_SPI
Definition: revo_f4.h:110
__STATIC_INLINE void NVIC_SystemReset(void)
System Reset.
Definition: core_cm0.h:647
void battery_voltage_set_multiplier(double multiplier) override
PWM_OUT esc_out_[PWM_NUM_OUTPUTS]
float read_voltage() const
Read the current battery voltage.
bool read_config(uint8_t *data, uint32_t len)
Definition: M25P16.cpp:60
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
void init(SPI *spi_drv)
Definition: mpu6000.cpp:52
uint64_t time
Definition: ublox.h:409
#define LED1_PIN
Definition: revo_f4.h:127
bool write_config(const uint8_t *data, const uint32_t len)
Definition: M25P16.cpp:77
bool battery_voltage_present() const override
#define SBUS_INV_GPIO
Definition: revo_f4.h:77
void writeUs(uint16_t value)
Definition: pwm.cpp:151
float battery_voltage_read() const override
void update()
Definition: mb1242.cpp:85
const uart_hardware_struct_t uart_config[NUM_UART]
Definition: revo_f4.h:64
int32_t height
Definition: ublox.h:413
uint32_t p_acc
Definition: ublox.h:429
const pwm_hardware_struct_t pwm_config[PWM_NUM_CHANNELS]
Definition: revo_f4.h:144
int32_t lat
Definition: ublox.h:411
int32_t vx
Definition: ublox.h:435
void serial_init(uint32_t baud_rate, uint32_t dev) override
bool backup_memory_read(void *dest, size_t len) override
void update()
Definition: hmc5883l.cpp:76
int32_t lon
Definition: ublox.h:412
virtual float read(uint8_t channel)=0
float rc_read(uint8_t channel) override
void disable()
Definition: pwm.cpp:135
void init(const ADCHardwareStruct *adc_def)
Initializes the ADC according to the definition struct provided.
uint32_t time_of_week
Definition: ublox.h:425
void init(I2C *_i2c)
Definition: mb1242.cpp:53
void on()
Definition: led.cpp:39
void init(SPI *_spi)
Definition: M25P16.cpp:38
void update()
Definition: ms5611.cpp:101
uint32_t v_acc
Definition: ublox.h:418
float battery_current_read() const override
bool new_data()
Definition: ublox.cpp:286
#define SBUS_INV_PIN
Definition: revo_f4.h:78
int32_t vel_d
Definition: ublox.h:416
bool present()
Definition: ublox.cpp:133
void backup_sram_write(const void *src, size_t len)
Definition: backup_sram.cpp:19
GNSSPosECEF read_pos_ecef()
Definition: ublox.cpp:310
void update()
Definition: ms4525.cpp:64
#define BARO_I2C
Definition: revo_f4.h:122
#define EXTERNAL_I2C
Definition: revo_f4.h:123
bool memory_read(void *dest, size_t len) override
void set_current_multiplier(double multiplier)
Sets the current multiplier for the battery monitor.
float read()
Definition: mb1242.cpp:103
void init(const pwm_hardware_struct_t *pwm_init, uint16_t frequency, uint32_t max_us, uint32_t min_us)
Definition: pwm.cpp:36
bool present()
Definition: ms5611.cpp:94
void init(const spi_hardware_struct_t *conf)
Definition: spi.cpp:40
virtual void write(const uint8_t *ch, uint8_t len)=0
void backup_sram_clear(size_t len)
Definition: backup_sram.cpp:37
int32_t vel_n
Definition: ublox.h:414
#define UART3
Definition: revo_f4.h:63
void mag_read(float mag[3]) override
bool battery_current_present() const override
bool init(I2C *i2c_drv)
Definition: hmc5883l.cpp:37
const spi_hardware_struct_t spi_config[NUM_SPI]
Definition: revo_f4.h:99
void init(const BatteryMonitorHardwareStruct &def, AnalogDigitalConverter *adc, float voltage_multiplier=0, float current_multiplier=0)
Initialize the battery monitor.
uint32_t h_acc
Definition: ublox.h:417
const NAV_PVT_t & read_full()
Definition: ublox.cpp:322
bool read(float mag_data[])
Definition: hmc5883l.cpp:100
void read(float *accel_data, float *gyro_data, float *temp_data, uint64_t *time_us)
Definition: mpu6000.cpp:118
void init(GPIO *inv_pin, UART *uart)
Definition: rc_sbus.cpp:42
bool init(I2C *_i2c)
Definition: ms4525.cpp:43
virtual uint32_t rx_bytes_waiting()=0
GNSSVelECEF read_vel_ecef()
Definition: ublox.cpp:316
void read(float *differential_pressure, float *temp)
Definition: ms4525.cpp:84
void check_connection_status()
Definition: ublox.cpp:71
void pwm_write(uint8_t channel, float value) override
void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override
int32_t vz
Definition: ublox.h:437
void serial_write(const uint8_t *src, size_t len) override
void off()
Definition: led.cpp:44
void board_reset(bool bootloader) override
const i2c_hardware_struct_t i2c_config[NUM_I2C]
Definition: revo_f4.h:116
void write(float value)
Definition: pwm.cpp:146
void rc_init(rc_type_t rc_type) override
void read(float *press, float *temp)
Definition: ms5611.cpp:384
bool new_data()
Definition: mpu6000.cpp:137
void battery_current_set_multiplier(double multiplier) override
void delay(uint32_t ms)
Definition: system.c:101
uint16_t serial_bytes_available() override
#define RC_PPM_PIN
Definition: revo_f4.h:276
GNSSPVT read()
Definition: ublox.cpp:291


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:15