sensor.c

/*
 * sensor.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include "sensor.h"
#include "communication.h"
#include "commands.h"

int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
{
	if(startMPU9150() == -1)
		return -1;

	// read sensor board and fill in gryo/accelerometer/magnetometer struct
	get_gam_reading(&(raw_sensor_struct->gam));

	// Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
	sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
	sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;

    return 0;
}

int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
{
//	///////// for testing update period of vrpn data from the ground station to the quad
//	static int update_counter = 0;
//	if(user_input_struct->hasPacket == 0x04)
//	{
//		char buf[200] = {};
//		// Send a reply to the ground station
//		snprintf(buf, sizeof(buf), "Received an update packet %dms\r\n", 5 * update_counter);
//		unsigned char *responsePacket;
//
//		metadata_t metadata =
//		{
//			BEGIN_CHAR,
//			MessageTypes[5].ID,
//			MessageTypes[5].subtypes[1].ID,
//			0,
//			(strlen(buf) + 1)
//		};
//		formatPacket(&metadata, buf, &responsePacket);
//
//		// Send each byte of the packet individually
//		int i;
//		for(i = 0; i < 8 + metadata.data_len; i++) {
//			// Debug print statement for all of the bytes being sent
////			printf("%d: 0x%x\n", i, responsePacket[i]);
//
//			uart0_sendByte(responsePacket[i]);
//		}
//		update_counter = 0;
//	}
//	else
//	{
//		update_counter++;
//	}
//
//	/////////// end testing


	// the the sensor board and fill in the readings into the GAM struct
	get_gam_reading(&(raw_sensor_struct->gam));

	log_struct->gam = raw_sensor_struct->gam;

    return 0;
}