Project 2 - Integrate following sensors to the Dev Board
PROGRAM :
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "queue_set.h"
#include "driverlib/uart.h"
#include "driverlib/i2c.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/interrupt.h"
#include "utils/uartstdio.h"
#include "gps.h"
#include "imu.h"
#include "utils.h"
// Queue handles
QueueHandle_t xQueueAccel, xQueueGyro;
QueueSetHandle_t xSensorSet;
// GPS Data Buffer
char gpsBuffer[128];
GPSData_t currentGPS, lastGPS;
// IMU Data Struct
SensorData_t latestAccel, latestGyro;
// Trigger threshold
#define DIST_THRESHOLD_METERS 100.0f
// GPS UART IRQ Handler
void GPS_UART_Handler(void) {
uint32_t status = UARTIntStatus(UART1_BASE, true);
UARTIntClear(UART1_BASE, status);
static int idx = 0;
while (UARTCharsAvail(UART1_BASE)) {
char c = UARTCharGetNonBlocking(UART1_BASE);
if (c == '\n') {
gpsBuffer[idx] = '\0';
GPS_Parse(gpsBuffer, ¤tGPS);
idx = 0;
} else {
gpsBuffer[idx++] = c;
if (idx >= sizeof(gpsBuffer)) idx = 0;
}
}
}
void vIMUTask(void *pv) {
SensorData_t acc, gyro;
while (1) {
if (IMU_ReadAccelerometer(&acc)) {
xQueueSend(xQueueAccel, &acc, 0);
}
if (IMU_ReadGyroscope(&gyro)) {
xQueueSend(xQueueGyro, &gyro, 0);
}
vTaskDelay(pdMS_TO_TICKS(100));
}
}
void vQueueHandlerTask(void *pv) {
QueueSetMemberHandle_t member;
SensorData_t sensor;
while (1) {
member = xQueueSelectFromSet(xSensorSet, portMAX_DELAY);
if (member == xQueueAccel) {
xQueueReceive(xQueueAccel, &sensor, 0);
latestAccel = sensor;
} else if (member == xQueueGyro) {
xQueueReceive(xQueueGyro, &sensor, 0);
latestGyro = sensor;
}
}
}
void vMonitorTask(void *pv) {
while (1) {
float distance = Haversine_Distance(lastGPS, currentGPS);
if (distance >= DIST_THRESHOLD_METERS) {
UARTprintf("\n[EVENT] Distance Changed > %.2f m\n", distance);
UARTprintf("Lat: %.4f Lon: %.4f\n", currentGPS.lat, currentGPS.lon);
UARTprintf("Accel: X=%.2f Y=%.2f Z=%.2f\n", latestAccel.x, latestAccel.y, latestAccel.z);
UARTprintf("Gyro : X=%.2f Y=%.2f Z=%.2f\n", latestGyro.x, latestGyro.y, latestGyro.z);
lastGPS = currentGPS;
}
vTaskDelay(pdMS_TO_TICKS(500));
}
}
void vSensorStatusTask(void *pv) {
bool gpsConnected = false, imuConnected = false;
while (1) {
if (GPS_CheckConnection()) {
if (!gpsConnected) {
UARTprintf("[INFO] GPS Connected\n");
gpsConnected = true;
}
} else {
if (gpsConnected) {
UARTprintf("[INFO] GPS Disconnected\n");
gpsConnected = false;
}
}
if (IMU_CheckConnection()) {
if (!imuConnected) {
UARTprintf("[INFO] IMU Connected\n");
imuConnected = true;
}
} else {
if (imuConnected) {
UARTprintf("[INFO] IMU Disconnected\n");
imuConnected = false;
}
}
vTaskDelay(pdMS_TO_TICKS(1000));
}
}
int main(void) {
// Clock setup
SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
UARTStdioConfig(0, 115200, SysCtlClockGet());
UARTprintf("System Booting...\n");
GPS_Init();
IMU_Init();
// Create queues and set
xQueueAccel = xQueueCreate(5, sizeof(SensorData_t));
xQueueGyro = xQueueCreate(5, sizeof(SensorData_t));
xSensorSet = xQueueCreateSet(10);
xQueueAddToSet(xQueueAccel, xSensorSet);
xQueueAddToSet(xQueueGyro, xSensorSet);
// Create tasks
xTaskCreate(vIMUTask, "IMU", 256, NULL, 2, NULL);
xTaskCreate(vQueueHandlerTask, "QueueHandler", 256, NULL, 2, NULL);
xTaskCreate(vMonitorTask, "Monitor", 256, NULL, 1, NULL);
xTaskCreate(vSensorStatusTask, "Status", 256, NULL, 1, NULL);
vTaskStartScheduler();
while (1);
}
---------------------------
OUTPUT :
[INFO] GPS Connected
[INFO] IMU Connected
[EVENT] Distance Changed > 100m
Lat: 28.6139 N Lon: 77.2090 E
Accel: X=0.12 Y=0.01 Z=9.81
Gyro : X=0.01 Y=0.02 Z=0.01
[INFO] IMU Disconnected
[INFO] GPS Disconnected