Acc / Gyro
Compass
Altitude
Ultrasonic

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)