multisense.c
Go to the documentation of this file.
1 /*
2  accelgyro.c : report accelerometer and gyroscope values
3 
4  Copyright (C) 2016 James Jackson
5 
6  This file is part of BreezySTM32.
7 
8  BreezySTM32 is free software: you can redistribute it and/or modify
9  it under the terms of the GNU General Public License as published by
10  the Free Software Foundation, either version 3 of the License, or
11  (at your option) any later version.
12 
13  BreezySTM32 is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU General Public License for more details.
17 
18  You should have received a copy of the GNU General Public License
19  along with BreezySTM32. If not, see <http://www.gnu.org/licenses/>.
20  */
21 
22 #include <breezystm32.h>
23 #include <stdint.h>
24 
25 #define BOARD_REV 2
26 
27 float accel_scale; // converts to units of m/s^2
28 float gyro_scale; // converts to units of rad/s
29 
30 int16_t accel_data[3];
31 int16_t gyro_data[3];
32 volatile int16_t temp_data;
33 float mag_data[3];
34 
35 volatile uint8_t accel_status = 0;
36 volatile uint8_t gyro_status = 0;
37 volatile uint8_t temp_status = 0;
38 volatile bool mpu_new_measurement = false;
39 
40 uint32_t start_time = 0;
41 
42 bool baro_present= false;
43 bool mag_present=false;
44 bool sonar_present=false;
45 bool airspeed_present=false;
46 void setup(void)
47 {
48  delay(500);
50 
51  // Init Baro
52  i2cWrite(0, 0, 0);
53  ms5611_init();
54 
55  // Init Mag
56  hmc5883lInit();
57 
58  // Init Sonar
59  mb1242_init();
60 
61  // Init Airspeed
62  ms4525_init();
63 
64  //Init IMU (has to come last because of the ISR)
65  uint16_t acc1G;
66  mpu6050_init(true, &acc1G, &gyro_scale, BOARD_REV);
67  accel_scale = 9.80665f / acc1G;
68 }
69 
70 void loop(void)
71 {
72 
73  float baro = 0;
74  float temp = 0;
75  float diff_pres = 0;
76  float diff_temp = 0;
77  float sonar = 0;
78 
79  // Update Baro
81  if(ms5611_present())
82  {
83  ms5611_async_read(&baro, &temp);
84  }
85 
86  // Update Mag
88  if(hmc5883l_present())
89  {
91  }
92 
93  // Update Sonar
95  if (mb1242_present())
96  {
97  sonar = mb1242_async_read();
98  }
99  // if(sonar_present)
100  // {
101  // sonar = mb1242_poll();
102  // }
103 
104  // Update Airspeed
106  if(ms4525_present())
107  {
108  ms4525_async_read(&diff_pres, &diff_temp);
109  }
110 
111  static uint64_t imu_timestamp_us = 0;
112  if (mpu6050_new_data())
113  {
114  mpu6050_async_read_all(accel_data, &temp_data, gyro_data, &imu_timestamp_us);
115  }
116 
117  static uint32_t last_print_ms = 0;
118  // Throttle printing
119  if(millis() > last_print_ms + 10)
120  {
121  last_print_ms += 10;
122 
123  printf("err: %d\t", i2cGetErrorCounter());
124  printf("acc: %d.%d, %d.%d, %d.%d\t", (int32_t)accel_data[0], (uint32_t)(accel_data[0]*100)%100,
125  (int32_t)accel_data[1], (uint32_t)(accel_data[1]*100)%100, (int32_t)accel_data[2], (uint32_t)(accel_data[2]*100)%100);
126  printf("baro: %d Pa %d.%d K\t", (uint32_t) baro, (int32_t) temp, (uint32_t)(temp*100)%100);
127  printf("mag: %d, %d, %d, \t", (int32_t) mag_data[0], (int32_t) mag_data[1], (int32_t) mag_data[2]);
128  printf("as: %d.%d, %d.%d\t", (int32_t)diff_pres, (uint32_t)(diff_pres*100)%100, (int32_t) diff_temp, (uint32_t)(diff_temp*100)%100);
129  printf("sonar: %d.%d", (int32_t)sonar, (uint32_t)(sonar*100)%100);
130 
131  printf("\n");
132 
133 // printf("%d\t %d\t %d\t %d\t %d\t %d\n",
136 // (int32_t)imu_timestamp_us,
137 // (int32_t)mag_data[2],
138 // (int32_t)baro,
139 // (int32_t)i2cGetErrorCounter());
140  }
141 }
uint16_t i2cGetErrorCounter(void)
Definition: drv_i2c.c:467
volatile uint8_t accel_status
Definition: multisense.c:35
bool hmc5883lInit()
Definition: drv_hmc5883l.c:83
bool ms4525_init()
Definition: drv_ms4525.c:40
int16_t accel_data[3]
Definition: multisense.c:30
void i2cInit(I2CDevice index)
Definition: drv_i2c.c:420
#define printf
Definition: printf.h:119
volatile uint8_t gyro_status
Definition: multisense.c:36
volatile uint32_t millis(void)
Definition: system.c:50
bool hmc5883l_present()
Definition: drv_hmc5883l.c:111
void mb1242_async_update()
Definition: drv_mb1242.c:76
bool sonar_present
Definition: multisense.c:44
bool ms5611_init()
Definition: drv_ms5611.c:151
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
volatile uint8_t temp_status
Definition: multisense.c:37
void loop(void)
Definition: multisense.c:70
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
bool airspeed_present
Definition: multisense.c:45
float gyro_scale
Definition: multisense.c:28
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
float accel_scale
Definition: multisense.c:27
bool ms4525_present()
Definition: drv_ms4525.c:62
bool mpu6050_new_data()
Definition: drv_mpu6050.c:328
float mag_data[3]
Definition: multisense.c:33
static volatile int16_t temp
Definition: drv_mpu6050.c:278
int16_t gyro_data[3]
Definition: multisense.c:31
void setup(void)
Definition: multisense.c:46
uint32_t start_time
Definition: multisense.c:40
#define BOARD_REV
Definition: multisense.c:25
void hmc5883l_request_async_update()
Definition: drv_hmc5883l.c:118
volatile int16_t temp_data
Definition: multisense.c:32
void ms5611_async_update()
Definition: drv_ms5611.c:405
bool ms5611_present()
Definition: drv_ms5611.c:203
float mb1242_async_read()
Definition: drv_mb1242.c:99
bool baro_present
Definition: multisense.c:42
volatile bool mpu_new_measurement
Definition: multisense.c:38
void ms4525_async_update()
Definition: drv_ms4525.c:53
bool mag_present
Definition: multisense.c:43
void delay(uint32_t ms)
Definition: system.c:98
void ms5611_async_read(float *pressure, float *temperature)
Definition: drv_ms5611.c:452


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:19