Project 2 - Integrate following sensors to the Dev Board
#include <stdio.h>
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "math.h"
#include "uart.h"
#include "i2c.h"
// UART and I2C handles (replace with actual peripheral handles)
UART_HandleTypeDef huart;
I2C_HandleTypeDef hi2c;
// Buffer and Queue Definitions
#define GPS_BUFFER_SIZE 256
char gpsBuffer[GPS_BUFFER_SIZE]; // Circular buffer for GPS data
volatile uint16_t gpsBufferIndex = 0;
QueueHandle_t accelQueue;
QueueHandle_t gyroQueue;
// GPS Coordinates (latitude, longitude)
float currentLatitude = 0.0, currentLongitude = 0.0;
float previousLatitude = 0.0, previousLongitude = 0.0;
// Haversine Distance Function (calculates distance between two GPS points in meters)
float calculateDistance(float lat1, float lon1, float lat2, float lon2) {
float dLat = (lat2 - lat1) * (M_PI / 180.0);
float dLon = (lon2 - lon1) * (M_PI / 180.0);
// Convert latitudes to radians
lat1 = lat1 * (M_PI / 180.0);
lat2 = lat2 * (M_PI / 180.0);
// Haversine formula
float a = sin(dLat / 2) * sin(dLat / 2) + cos(lat1) * cos(lat2) * sin(dLon / 2) * sin(dLon / 2);
float c = 2 * atan2(sqrt(a), sqrt(1 - a));
float radius = 6371000; // Earth’s radius in meters
return radius * c;
}
// Task to handle GPS data reception
void vGPSReceiverTask(void *pvParameters) {
char gpsData[GPS_BUFFER_SIZE];
for (;;) {
// Wait for UART interrupt (mocked for example)
if (gpsBufferIndex > 0) {
// Copy buffer and reset index
strcpy(gpsData, gpsBuffer);
gpsBufferIndex = 0;
// Parse GPS data for latitude and longitude (implementation depends on GPS sentence format)
// Extract latitude and longitude from NMEA sentence (assuming GPRMC sentence format)
// Example: $GPRMC,HHMMSS,A,latitude,N,longitude,E,...
// Update currentLatitude and currentLongitude based on parsed data
// Assuming gps_parse_coordinates() is a function to parse GPS sentences
if (gps_parse_coordinates(gpsData, ¤tLatitude, ¤tLongitude)) {
float distance = calculateDistance(previousLatitude, previousLongitude, currentLatitude, currentLongitude);
if (distance >= 100.0) {
// Print GPS coordinates and trigger IMU read
printf("Moved > 100m: Lat: %.6f, Lon: %.6f\n", currentLatitude, currentLongitude);
previousLatitude = currentLatitude;
previousLongitude = currentLongitude;
// Trigger IMU data request
vTaskNotifyGive(imuTaskHandle);
}
}
}
// Delay for some time (or use event-triggered wait in case of UART interrupt)
vTaskDelay(pdMS_TO_TICKS(500));
}
}
// Task to handle IMU data reception and process accelerometer/gyroscope data
void vIMUTask(void *pvParameters) {
int16_t accelData[3], gyroData[3];
for (;;) {
// Wait for notification from the GPS task to process IMU data
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
// Read Accelerometer data over I2C
if (HAL_I2C_Mem_Read(&hi2c, ACCEL_ADDRESS, ACCEL_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t *)accelData, sizeof(accelData), HAL_MAX_DELAY) == HAL_OK) {
xQueueSend(accelQueue, accelData, portMAX_DELAY);
}
// Read Gyroscope data over I2C
if (HAL_I2C_Mem_Read(&hi2c, GYRO_ADDRESS, GYRO_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t *)gyroData, sizeof(gyroData), HAL_MAX_DELAY) == HAL_OK) {
xQueueSend(gyroQueue, gyroData, portMAX_DELAY);
}
// Retrieve data from queues and print to UART
if (xQueueReceive(accelQueue, &accelData, portMAX_DELAY)) {
printf("Accelerometer: X=%d, Y=%d, Z=%d\n", accelData[0], accelData[1], accelData[2]);
}
if (xQueueReceive(gyroQueue, &gyroData, portMAX_DELAY)) {
printf("Gyroscope: X=%d, Y=%d, Z=%d\n", gyroData[0], gyroData[1], gyroData[2]);
}
}
}
// Task to monitor GPS and IMU connection status
void vConnectionMonitorTask(void *pvParameters) {
for (;;) {
// Mock connection detection (replace with actual GPIO or bus error detection logic)
if (isGPSConnected()) {
printf("GPS Connected\n");
} else {
printf("GPS Disconnected\n");
}
if (isIMUConnected()) {
printf("IMU Connected\n");
} else {
printf("IMU Disconnected\n");
}
// Monitor periodically
vTaskDelay(pdMS_TO_TICKS(2000)); // Check every 2 seconds
}
}
// Main function to create tasks and start FreeRTOS scheduler
int main(void) {
// Initialize UART and I2C (replace with actual HAL init code)
HAL_UART_Init(&huart);
HAL_I2C_Init(&hi2c);
// Create queues for accelerometer and gyroscope data
accelQueue = xQueueCreate(10, sizeof(int16_t[3]));
gyroQueue = xQueueCreate(10, sizeof(int16_t[3]));
// Create tasks
xTaskCreate(vGPSReceiverTask, "GPS Receiver", 512, NULL, 1, NULL);
xTaskCreate(vIMUTask, "IMU Task", 512, NULL, 1, &imuTaskHandle);
xTaskCreate(vConnectionMonitorTask, "Connection Monitor", 256, NULL, 1, NULL);
// Start the scheduler
vTaskStartScheduler();
// Should never reach here
for (;;) {}
return 0;
}
// UART receive interrupt handler (mocked for example)
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
// Store received data in the GPS buffer
gpsBuffer[gpsBufferIndex++] = receivedByte;
if (gpsBufferIndex >= GPS_BUFFER_SIZE) {
gpsBufferIndex = 0; // Wrap around
}
// Restart UART receive interrupt
HAL_UART_Receive_IT(&huart, &receivedByte, 1);
}