Project 2 - Integrate following sensors to the Dev Board

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <stdint.h>

#include <stdbool.h>

#include <FreeRTOS.h>

#include <task.h>

#include <queue.h>

// Define UART and I2C communication parameters

#define GPS_UART_PORT 0

#define IMU_I2C_PORT 1

// Define GPS UART buffer size

#define GPS_UART_BUFFER_SIZE 128

// Define task priorities

#define GPS_TASK_PRIORITY (configMAX_PRIORITIES - 1)

#define IMU_TASK_PRIORITY (configMAX_PRIORITIES - 2)

#define MAIN_TASK_PRIORITY (configMAX_PRIORITIES - 3)

// Define distance change threshold (100 meters)

#define DISTANCE_CHANGE_THRESHOLD 100

// Define queue length

#define QUEUE_LENGTH 10

// Define queue handles

QueueHandle_t accelerometerQueue;

QueueHandle_t gyroscopeQueue;

// Function prototypes

void gpsTask(void *pvParameters);

void imuTask(void *pvParameters);

void mainTask(void *pvParameters);

// Simulated GPS data structure

typedef struct {

double latitude;

double longitude;

} GPSData;

// Simulated IMU data structure

typedef struct {

double accelerometer[3]; // X, Y, Z

double gyroscope[3]; // X, Y, Z

} IMUData;

// Function to read GPS data from UART

void readGPSData(char *buffer, size_t length) {

// Simulated GPS data

snprintf(buffer, length, "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");

}

// Function to read IMU data from I2C

void readIMUData(IMUData *data) {

// Simulated IMU data

data->accelerometer[0] = 1.0;

data->accelerometer[1] = 2.0;

data->accelerometer[2] = 3.0;

data->gyroscope[0] = 0.1;

data->gyroscope[1] = 0.2;

data->gyroscope[2] = 0.3;

}

// Function to calculate distance between two GPS coordinates

double calculateDistance(GPSData *prevGPSData, GPSData *currentGPSData) {

// Simulated distance calculation

return 50.0; // Placeholder value

}

// GPS task

void gpsTask(void *pvParameters) {

char buffer[GPS_UART_BUFFER_SIZE];

GPSData prevGPSData, currentGPSData;

bool gpsConnected = false;

while (1) {

// Read GPS data from UART

readGPSData(buffer, sizeof(buffer));

// Parse GPS data (dummy implementation)

// Assuming GPS data format: latitude,longitude

sscanf(buffer, "%lf,%lf", &currentGPSData.latitude, &currentGPSData.longitude);

// Check if GPS is connected

if (!gpsConnected) {

printf("GPS Connected\n");

gpsConnected = true;

}

// Calculate distance change

double distance = calculateDistance(&prevGPSData, &currentGPSData);

// Check if distance change is greater than threshold

if (distance > DISTANCE_CHANGE_THRESHOLD) {

// Read IMU data

IMUData imuData;

readIMUData(&imuData);

// Print GPS and IMU data

printf("GPS Coordinates: (%lf, %lf)\n", currentGPSData.latitude, currentGPSData.longitude);

printf("Accelerometer: (%lf, %lf, %lf)\n", imuData.accelerometer[0], imuData.accelerometer[1], imuData.accelerometer[2]);

printf("Gyroscope: (%lf, %lf, %lf)\n", imuData.gyroscope[0], imuData.gyroscope[1], imuData.gyroscope[2]);

}

// Update previous GPS data

prevGPSData = currentGPSData;

vTaskDelay(pdMS_TO_TICKS(1000)); // 1 second delay

}

}

// IMU task

void imuTask(void *pvParameters) {

while (1) {

// Read IMU data

IMUData imuData;

readIMUData(&imuData);

// Separate accelerometer and gyroscope data

xQueueSend(accelerometerQueue, &imuData.accelerometer, portMAX_DELAY);

xQueueSend(gyroscopeQueue, &imuData.gyroscope, portMAX_DELAY);

vTaskDelay(pdMS_TO_TICKS(100)); // 100 ms delay

}

}

// Main task

void mainTask(void *pvParameters) {

// Create GPS task

xTaskCreate(gpsTask, "GPSTask", configMINIMAL_STACK_SIZE, NULL, GPS_TASK_PRIORITY, NULL);

// Create IMU task

xTaskCreate(imuTask, "IMUTask", configMINIMAL_STACK_SIZE, NULL, IMU_TASK_PRIORITY, NULL);

// Main task does not need to do anything

while (1) {

vTaskDelay(pdMS_TO_TICKS(1000)); // 1 second delay

}

}

int main() {

// Create queues for accelerometer and gyroscope data

accelerometerQueue = xQueueCreate(QUEUE_LENGTH, sizeof(double[3]));

gyroscopeQueue = xQueueCreate(QUEUE_LENGTH, sizeof(double[3]));

// Create main task

xTaskCreate(mainTask, "MainTask", configMINIMAL_STACK_SIZE, NULL, MAIN_TASK_PRIORITY, NULL);

// Start FreeRTOS scheduler

vTaskStartScheduler();

// Should never reach here

return 0;

}

We have three tasks: gpsTask, imuTask, and mainTask.

gpsTask reads GPS data from UART, calculates distance change, and prints GPS and IMU data when the distance change is greater than the threshold.

imuTask reads IMU data from I2C, separates accelerometer and gyroscope data, and sends them to separate queues.

mainTask creates the other tasks and then enters an idle loop.

We simulate GPS and IMU data reading functions (readGPSData and readIMUData). You should replace these with functions suitable for your hardware and library.

Queue handles accelerometerQueue and gyroscopeQueue are used to transfer accelerometer and gyroscope data to the main task for processing.

The UART communication with the GPS sensor and I2C communication with the IMU sensor should be properly initialized in your hardware setup.