I2C Bus Initialization
Initialize
Accelorometer Get X
Gyroscope Get Z
Gyroscope Get X
Accelorometer Get Y
Accelorometer Get Z
Gyroscope Get Y
Set Altitude
Get Altitude
Altitude Activate
Altitude Deactivate
Get Heading (in degrees)
Get Distance (in mm)
Set Heading (in degrees)
Compass Activate
MPU6050 (accel / gyro)
Accelorometer Set X
BMP280 (Pressure)
Accelorometer Set Z
Gyroscope Set X
QMC5883L (compass)
HCSR04 (ultrasonic)
Accelorometer Set Y
Read sensor
Read sensor
Compass Deactivate
if(mpu is active)
Read sensor
if(bmp is active)
Read sensor
if(error) error_count++
if(error) error_count++
if(error) error_count++
if(error) error_count++
Accelorometer Activate
Accelorometer Deactivate
#define MAX_ERROR_COUNT
int error_count = 0
Ultrasonic Activate
Ultrasonic Deactivate
sensor manager task
Reboot Devices
if(qmc is active)
if(hcsr is active)
Gyroscope Set Z
Gyroscope Set Y
Gyroscope Deactivate
Gyroscope Activate
while(error_count < MAX_ERROR_COUNT)
while(true) // loop forever
NOTE The sensor manager task will make sure that the I2C bus is up and running and that all sensors are properly functioning.. It will then be used to constantly read from sensors (when told to)