uint8_t processUserInput(int8_t opt) {
char msg[30];
if(!(opt >=1 && opt <= 3))
return 0;
sprintf(msg, "%d", opt);
UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg));
switch(opt) {
case 1:
HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
break;
case 2:
sprintf(msg, "\r\nUSER BUTTON status: %s",
HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_13) == GPIO_PIN_RESET ? "PRESSED" : "RELEASED");
UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg));
break;
case 3:
return 2;
};
UART_Transmit(&huart2, (uint8_t*)PROMPT, strlen(PROMPT));
return 1;
}
//print new line
void PrintLine()
{
setTimerIn4ms(3);
while(!timerExpired());
UART_Transmit(13);
setTimerIn4ms(3);
while(!timerExpired());
UART_Transmit(10);
}
//print text and new line
void PrintLn(char Text[80])
{
unsigned char Len, i, T;
Len = Length(Text);
for (i=0; i<Len; i++)
{
T = Text[i];
UART_Transmit(T);
}
UART_Transmit(13);
UART_Transmit(10);
}
uint8_t BSP_FP_DeleteAll(void)
{
uint8_t DataBuf[25]={0},RxBuf[25]={0};
DataBuf[0]=0x7e;
DataBuf[4]=0x23; //command stream to fingerprint
DataBuf[24]=0x23; //0x7e,0,0,0,0x37,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x37
UART_Transmit(&DataBuf,25,10); //send command
UART_Receive(&RxBuf,25,5000); //receive acknoledgement
/**
*RxBuf[4]==0x23 : Received command must be same as transmitted command
*RxBuf[8]==0x01 : Stands for successful execution of transmitted command
*If not executed successfully RxBuf[8] is used to determine error status
*/
if((RxBuf[4] == 0x23) && (RxBuf[8] == 0x01))
{
return(RESULT_SUCCEEDED);
}
else
{
return(RxBuf[8]);
}
}
/**
* @brief This function scans the finger print for comparing with previously
* stored Finger Prints On identification it displays the correct Id No.
* @param[in] pointer to the id no. at which matched id no. will be stored.
* @return connection status
*/
uint8_t BSP_FP_Identify(uint8_t *id)
{
uint8_t DataBuf[25]={0},RxBuf[25]={0},XtraBuf[14],i;
DataBuf[0]=0x7E;
DataBuf[4]=0x12; //command stream to fingerprint
DataBuf[24]=0x12; //0x7e,0,0,0,0x12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x12
UART_Transmit(&DataBuf,25,10); //send command
UART_Receive(&RxBuf,25,5000); //receive acknowledgment
if(RxBuf[16]!=0) //if Data size is not zero then read data
{
UART_Receive(&XtraBuf,14,5000);
id[0]=XtraBuf[0];
id[1]=XtraBuf[1];
id[2]=XtraBuf[2];
id[3]=XtraBuf[3];
}
/**
*RxBuf[4]==0x12 : Received command must be same as transmitted command
*RxBuf[8]==0x01 : Stands for successful execution of transmitted command
*If not executed successfully RxBuf[8] is used to determine error status
*/
if((RxBuf[4] == 0x12) && (RxBuf[8] == 0x01))
{
return(RESULT_SUCCEEDED);
}
else
{
return(RxBuf[8]);
}
}
void ReceiveComplete() {
p++;
if (l) {
//erst rom
//if (UartDaten[0] != 0x28) { //fals doch daten gesendet wurden
// UART_Receive(&TempUart, UartDaten, 8);
//return;
//}
k = indexFestlegen(UartRom);
UART_Receive(&TempUart, UartDaten, 8);
l = 0;
} else {
// dann daten
// if (UartDaten[0] == 0x2 && UartDaten[1] == 0x8) { //fals doch adresse gesendet wurde
// UART_Receive(&TempUart, UartDaten, 8);
// return;
// }
UART_Receive(&TempUart, UartRom, 8);
l = 1;
datenAuswerten(UartDaten);
n++;
if (n > 5) {
UART_Transmit(&TempUart, &o, 1);
n = 0;
}
}
}
/**
* @brief This function is used to get connection with finger print device
* @param[in] none
* @return connection status
*/
uint8_t BSP_FP_Connect(void)
{
uint8_t i,DataBuf[25]={0},RxBuf[25]={0};
DataBuf[0]=0x7E;
DataBuf[4]=0x01; //command stream to fingerprint
DataBuf[24]=0x01; //0x7e,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
UART_Transmit(&DataBuf,25,10);
UART_Receive(&RxBuf,25,100);
/**
*RxBuf[4]==0x01 : Received command must be same as transmitted command
*RxBuf[8]==0x01 : Stands for successful execution of transmitted command
*If not executed successfully RxBuf[8] is used to determine error status
*/
if((RxBuf[4] == 0x01) && (RxBuf[8] == 0x01))
{
return(RESULT_SUCCEEDED);
}
else
{
return(RxBuf[8]);
}
}
void printWelcomeMessage(void) {
char *strings[] = {"\033[0;0H", "\033[2J", WELCOME_MSG, MAIN_MENU, PROMPT};
for (uint8_t i = 0; i < 5; i++) {
UART_Transmit(&huart2, (uint8_t*)strings[i], strlen(strings[i]));
while (HAL_UART_GetState(&huart2) == HAL_UART_STATE_BUSY_TX || HAL_UART_GetState(&huart2) == HAL_UART_STATE_BUSY_TX_RX);
}
}
void State_Run(uint32_t ticks, OS_t *os){
uint8_t received_bytes[32];
uint8_t received_length;
POINTER_TO_CONTAINER(RC_Data_t, rc_data);
POINTER_TO_CONTAINER(Motorcontrolvalues_t, motors);
POINTER_TO_CONTAINER(Vector_i32_t, helper_speed);
uint8_t send_bytes[37];
format_set_u8_buf_to(0, send_bytes, 36);
send_bytes[36] = '\n';
if((ticks % 20) == 0){
received_length = RFM75_Receive_bytes(received_bytes);
if(received_length == 0){
rc_no_data_receive_count++;
}
delay_ms(2);
Vect_i32_set_all_values_to(helper_speed, 0);
Motion_sensor_get_data(os->motion_sensor);
Vect_i32_times_const(&start_offset.angle_speed, -1, helper_speed);
Vect_i32_add(&os->motion_sensor->angle_speed, helper_speed, helper_speed);
integrator_count++;
/* Now helper speed has the value without offset */
Vect_i32_copy_from_to(helper_speed, &angular_speeds[integrator_count]);
if(integrator_count >= 2){
Vect_i32_times_const(&angular_speeds[1], 4, helper_speed);
Vect_i32_add_to(helper_speed, &angular_speeds[2]);
Vect_i32_add_to(helper_speed, &angular_speeds[0]);
Vect_i32_div_by_const(helper_speed, 6, helper_speed);
Vect_i32_add_to(&angular_position, helper_speed);
Vect_i32_copy_from_to(&angular_speeds[2], &angular_speeds[0]);
integrator_count = 0;
}
if(received_length == 16){
RC_Control_decode_message(received_bytes, rc_data);
Motors_set_all_data_speed(motors, rc_data->throttle/4);
led_toggle(LED1);
rc_no_data_receive_count = 0;
}
if(rc_no_data_receive_count > 20){
Motors_set_all_data_speed(motors, 0);
}
Motors_act_on_pwm(motors);
serialize_vector_as_i32(&os->motion_sensor->acceleration, &send_bytes[0]);
serialize_vector_as_i32(&os->motion_sensor->angle_speed, &send_bytes[12]);
serialize_vector_as_i32(&os->motion_sensor->magnetic_field, &send_bytes[24]);
UART_Transmit(&DebugUart, send_bytes, 37);
}
if((ticks % 500) == 0){
led_toggle(LED1);
}
}
void PrintStatus(unsigned char T)
{
// convert the value to HEX value in ASCII characters
unsigned char hex[2]={'0','0'};
unsigned char hexArray[]={'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
int i=1;
while(T>0)
{
hex[i]=T%16;
hex[i]=hexArray[hex[i]];
T=floor(T/16);
i--;
}
UART_Transmit('$'); //output value of T
UART_Transmit(hex[0]);
UART_Transmit(hex[1]);
PrintLine();
}
//print text
void Print(char Text[40])
{
unsigned char Len, i, T;
Len = Length(Text);
for (i=0; i<Len; i++)
{
T = Text[i];
UART_Transmit(T);
}
strcpy(Text, "");
}
//print text and new line
void PrintLn(char Text[40])
{
unsigned char Len, i, T;
Len = Length(Text);
for (i=0; i<Len; i++)
{
T = Text[i];
setTimerIn4ms(3);
while(!timerExpired());
UART_Transmit(T);
}
setTimerIn4ms(3);
while(!timerExpired());
UART_Transmit(13);
setTimerIn4ms(3);
while(!timerExpired());
UART_Transmit(10);
}
// Overall Current Function Definition
void Currentfunct(void)
{
// The binary representation of the frame for
// displaying a "A" on the CPLD.
uint8_t CurrentFrame = 0b11001011;
// Send the "A" frame for 1 second.
setTimerDelay(1);
PORTB |= (1<<5);
while(1)
{
// If the overflow is raised, then the 1 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
UART_Transmit(CurrentFrame);
}
// Checking if the signal on pin 3 is above a certain
// value. If it is below that value, then we will change
// to reading from the higher gain circuit.
uint8_t adcPin = CurrentPinSelect(PC3_IsenLG);
float currentPk = CurrentMax(adcPin);
//float currentRMS = RMSval(adcPin,2);
// Current in mA
currentPk = currentPk * 1000;
//
setTimerDelay(2);
PORTB &= ~(1<<5);
while(1)
{
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
MakeArray_Trans(currentPk);
}
}
// Energy Calculation, Transmission and Storage function
void Energyfunct(void)
{
// The binary representation of the frame for
// displaying a "E" on the CPLD.
uint8_t EnergyFrame = 0b11101010;
// Send the "E" frame for 1 second.
setTimerDelay(1);
PORTB |= (1<<5);
while(1)
{
// If the overflow is raised, then the 1 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
UART_Transmit(EnergyFrame);
}
// Storing the energy value into eeprom.
eeprom_add(energy);
// EnergyVal is new energy value
uint16_t EngyVal = energy;
// This energy value is then given to the MakeArray_Trans
// function to be transmitted for 2 seconds.
setTimerDelay(2);
PORTB &= ~(1<<5);
while(1)
{
// If the overflow is raised, then the 2 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
MakeArray_Trans(EngyVal);
}
}
// Overall Voltage Function Definition
void Voltagefunct(void)
{
// The binary representation of the frame for
// displaying a "V" on the CPLD.
uint8_t VoltageFrame = 0b11001010;
// Send the "V" frame for 1 second.
setTimerDelay(1);
PORTB |= (1<<5);
while(1)
{
// If the overflow is raised, then the 1 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
UART_Transmit(VoltageFrame);
}
// Telling the function to calculate the Vload
// RMS value using PIN C1
float VLoad = RMSval(PC1_Vsen, 1);
// This RMS value is then given to the MakeArray_Trans
// function to be transmitted for 2 seconds.
setTimerDelay(2);
PORTB &= ~(1<<5);
while(1)
{
// If the overflow is raised, then the 2 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
MakeArray_Trans(VLoad);
}
}
void UART_Main()
{
if(UART->RxGO)
{
UART->RxGO=0;
UART_Receive();
setSegmentDisplayNumber(UART->RxData[5]);
RCIE=1;
}
else
{
if(UART->TxGO)
{
UART->TxGO=0;
UART_Transmit();
}
}
}
void commSendData(void)
{
// Function that sends a new data set over UART to the RasPi
// Message format
//
// [0] compositeSlaveAddress | [1-4] Reading | [5-8] TimeStamp | [9] TimeStamp Set | [10-11] '\r\n'
//
uint8_t commData[] = {0,0,0,0,0,0,0,0,0,0,'\r','\n'}; // Buffer: Data to be sent
dequeue(commData); // Call the dequeue function defined in storage.c to get
// the next data set to be sent.
UART_Transmit(&UART_1,commData,12); // Add the data to the TX buffer.
commReadyForWrite = 0; // Clear the readForWrite flag to show that a TC
// operation is currently ongoing.
}
uint8_t BSP_FP_Delete(uint8_t *id)
{
uint8_t checksum= 0xc0,DataBuf[25]={0},RxBuf[25]={0};
int8_t i=0;
DataBuf[0]=0x7E;
DataBuf[4]=0x22;
DataBuf[16]=0x0A; //command stream to fingerprint
DataBuf[24]=0x2C; //0x7e,0,0,0,0x22,0,0,0,0,0,0,0,0,0,0,0,0x0a,0,0,0,0,0,0,0,0x2c
UART_Transmit(&DataBuf,25,10);
for(i=3;i>=0;i--)
{
uprintf("%c",id[i]); //send ascii value of id no. digit by digit
checksum+=id[i] - 0x30;
}
for(i=0;i<9;i++)
{
uprintf("%c",0x0); //send command
}
uprintf("%c",checksum); //send checksum
UART_Receive(&RxBuf,25,5000); //receive acknowledgment
/**
*RxBuf[4]==0x22 : Received command must be same as transmitted command
*RxBuf[8]==0x01 : Stands for successful execution of transmitted command
*If not executed successfully RxBuf[8] is used to determine error status
*/
if((RxBuf[4] == 0x22) && (RxBuf[8] == 0x01))
{
return(RESULT_SUCCEEDED);
}
else
{
return(RxBuf[8]);
}
}
uint8_t BSP_FP_List(uint8_t *database, uint8_t *count)
{
uint8_t DataBuf[25]={0},RxBuf[25]={0},XtraBuf[998]={0},i,t;
DataBuf[0]=0x7E;
DataBuf[4]=0x30; //command stream to fingerprint
DataBuf[24]=0x30; //0x7e,0,0,0,0x12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x12
UART_Transmit(&DataBuf,25,10); //send command
UART_Receive(&RxBuf,25,5000); //receive acknowledgment
UART_Receive(&XtraBuf,(RxBuf[16]+4),5000); //receive acknowledgment
*count=0;
for(i=0;i<100;i++)
database[i]=0;
if(RxBuf[16]>8)
{
for(i=4;i<(RxBuf[16]);i+=10)
{
t=((XtraBuf[i+2]-0x30)*10+(XtraBuf[i+3]-0x30));
database[t]=1; //storing database.
*count=*count+1;
}
}
/**
*RxBuf[4]==0x30 : Received command must be same as transmitted command
*RxBuf[8]==0x01 : Stands for successful execution of transmitted command
*If not executed successfully RxBuf[8] is used to determine error status
*/
if((RxBuf[4] == 0x30) && (RxBuf[8] == 0x01))
{
return(RESULT_SUCCEEDED);
}
else
{
return(RxBuf[8]);
}
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
SysTick_Config(SystemCoreClock / 1000);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_ResetBits(GPIOE, GPIO_Pin_2); //start with gps off to make sure it activates when wanted
GPIO_Start();
ADC_Start();
Flash_Start();
unsigned long tickey = getSysTick()+1000;
GPIO_ResetBits(GPIOA, GPIO_Pin_10); //LCD Reset must be held 10us
GPIO_SetBits(GPIOG, GPIO_Pin_3); //flash deselect
GPIO_SetBits(GPIOC, GPIO_Pin_8); //flash #hold off, we have dedicated pins
GPIO_SetBits(GPIOC, GPIO_Pin_1); //osc enable
GPIO_ResetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOE, GPIO_Pin_6); //buck enable
while(getSysTick()<tickey);
GPIO_SetBits(GPIOE, GPIO_Pin_2); //gps on/off
GPIO_SetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOA, GPIO_Pin_10); //LCD unreset
UART4_Start();
UART5_Start();
MPU_Start();
//========================BUTTONS====================
InitButton(&button1, GPIOE, GPIO_Pin_4);
#ifdef BOARD_V1
InitButton(&button2, GPIOE, GPIO_Pin_5);
#else
InitButton(&button2, GPIOA, GPIO_Pin_9);
#endif
//=======================END BUTTONS==================
/* LCD Configuration */
LCD_Config();
/* Enable The LCD */
LTDC_Cmd(ENABLE);
LCD_SetLayer(LCD_FOREGROUND_LAYER);
GUI_ClearBackground();
int count = 0;
delay(20000);
#ifndef ORIGIN
GUI_InitNode(1, 72, 83, 0xe8ec);
GUI_InitNode(2, 86, 72, 0xfd20);
GUI_InitNode(3, 'R', 'F', 0x001f);
#endif
int screencount = 0;
#ifdef INSIDE
origin_state.lati=KRESGE_LAT;
origin_state.longi=KRESGE_LONG;
origin_state.gpslock=1;
#endif
unsigned long tickey2 = getSysTick()+2000; //2 second counter
unsigned long tickey3 = getSysTick()+4000; //4 second delay to check gps state
/* Infinite loop */
while (1)
{
UpdateButton(&button1);
UpdateButton(&button2);
if( buttonRisingEdge(&button1)){//right
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);//yellow
//UART_Transmit(&huart4, gps_init_msg, cmdData1Len, 500);
origin_state.pingnum+=1;
origin_state.pingactive=1;
origin_state.whodunnit = origin_state.id;
origin_state.pingclearedby = 0;
}
if(buttonRisingEdge(&button2)){//left
//UART_Transmit(&huart4, gps_get_time_msg, cmdData2Len, 500);
GPIO_ToggleBits(GPIOA, GPIO_Pin_2); //green
if(origin_state.pingactive&&(origin_state.whodunnit != origin_state.id)){
origin_state.pingactive=0;
}
}
if(origin_state.gpson>2 &&(getSysTick()>tickey3)){
GPIO_ResetBits(GPIOE, GPIO_Pin_2);
delay(20000);
GPIO_SetBits(GPIOE, GPIO_Pin_2);
delay(20000);
char setme[80];
sprintf(setme, "%s%c%c", gps_init_msg, 0x0D, 0x0A);
UART_Transmit(UART4, setme, sizeof(setme)/sizeof(setme[0]), 5000);
origin_state.gpson=0;
tickey3+=4000;
}
if(getReset()){
NVIC_SystemReset();
}
#ifdef ORIGIN
// long actHeading=0;
// inv_get_sensor_type_heading(&actHeading, &headingAcc, &headingTime);
// degrees=((double)actHeading)/((double)65536.0);
// origin_state.heading=degrees;
long actHeading[3] = {0,0,0};
inv_get_sensor_type_euler(actHeading, &headingAcc, &headingTime);
degrees=((double)actHeading[2])/((double)65536.0);
//origin_state.heading=degrees;
long tempyraiture;
mpu_get_temperature(&tempyraiture, NULL);
// short garbage[3];
// mpu_get_compass_reg(garbage, NULL);
// double compass_angle = atan2(-garbage[0], -garbage[1])*180/3.1415;
// //origin_state.heading = .9*degrees + .1*compass_angle;
// origin_state.heading = compass_angle;
#endif
if(getSysTick()>tickey2){
tickey2 +=2000;
sendMessage();
}
processGPS();
processXbee();
if(getSysTick()>tickey){
tickey +=53;
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);
#ifndef ORIGIN
GUI_UpdateNode(1, degrees*3.1415/180.0+3.14*1.25, screencount, (screencount>10), 0);
GUI_UpdateNode(2, degrees*3.1415/180.0+3.14, screencount, (screencount>30), 0);
GUI_UpdateNode(3, degrees*3.1415/180.0+0, screencount, (screencount>50), 0);
#else
GUI_UpdateNodes();
#endif
GUI_UpdateArrow(-degrees*3.1415/180.0);
GUI_UpdateBattery(getBatteryStatus());
GUI_DrawTime();
if (count > 50){
GUI_UpdateBottomButton(1, 0xe8ec);
} else {
GUI_UpdateBottomButton(0, 0);
}
GUI_Redraw();
screencount += 1;
#ifndef ORIGIN
degrees += 3.6;
if (screencount%100 == 0){
screencount = 0;
degrees = 0;
}
#else
if (screencount%100 == 0){
screencount = 0;
}
#endif
}
//Sensors_I2C_ReadRegister((unsigned char)0x68, (unsigned char)MPU_WHOAMI, 1, inImu);
//==================================IMU================================
unsigned long sensor_timestamp;
int new_data = 0;
get_tick_count(×tamp);
#ifdef COMPASS_ENABLED
/* We're not using a data ready interrupt for the compass, so we'll
* make our compass reads timer-based instead.
*/
if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
hal.new_gyro && (hal.sensors & COMPASS_ON)) {
hal.next_compass_ms = timestamp + COMPASS_READ_MS;
new_compass = 1;
}
#endif
/* Temperature data doesn't need to be read with every gyro sample.
* Let's make them timer-based like the compass reads.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.motion_int_mode) {
/* Enable motion interrupt. */
mpu_lp_motion_interrupt(500, 1, 5);
/* Notify the MPL that contiguity was broken. */
inv_accel_was_turned_off();
inv_gyro_was_turned_off();
inv_compass_was_turned_off();
inv_quaternion_sensor_was_turned_off();
/* Wait for the MPU interrupt. */
while (!hal.new_gyro) {}
/* Restore the previous sensor configuration. */
mpu_lp_motion_interrupt(0, 0, 0);
hal.motion_int_mode = 0;
}
if (!hal.sensors || !hal.new_gyro) {
continue;
}
if (hal.new_gyro && hal.lp_accel_mode) {
short accel_short[3];
long accel[3];
mpu_get_accel_reg(accel_short, &sensor_timestamp);
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
hal.new_gyro = 0;
} else if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
inv_build_quat(quat, 0, sensor_timestamp);
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel_short[3];
unsigned char sensors, more;
long accel[3], temperature;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
}
#ifdef COMPASS_ENABLED
if (new_compass) {
short compass_short[3];
long compass[3];
new_compass = 0;
/* For any MPU device with an AKM on the auxiliary I2C bus, the raw
* magnetometer registers are copied to special gyro registers.
*/
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
compass[0] = (long)compass_short[0];
compass[1] = (long)compass_short[1];
compass[2] = (long)compass_short[2];
/* NOTE: If using a third-party compass calibration library,
* pass in the compass data in uT * 2^16 and set the second
* parameter to INV_CALIBRATED | acc, where acc is the
* accuracy from 0 to 3.
*/
inv_build_compass(compass, 0, sensor_timestamp);
}
new_data = 1;
}
#endif
if (new_data) {
inv_execute_on_data();
/* This function reads bias-compensated sensor data and sensor
* fusion outputs from the MPL. The outputs are formatted as seen
* in eMPL_outputs.c. This function only needs to be called at the
* rate requested by the host.
*/
read_from_mpl();
}
//========================================IMU==================================
}
}
// Overall Power Function Definition
void Powerfunct(void)
{
// The binary representation of the frame for
// displaying a "P" on the CPLD.
uint8_t PowerFrame = 0b11101000;
// Send the "P" frame for 1 second.
setTimerDelay(1);
PORTB |= (1<<5);
while(1)
{
// If the overflow is raised, then the 1 second
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// If the overflow isn't raised then keep transmitting
UART_Transmit(PowerFrame);
}
// Checking if the signal on pin 3 is above a certain
// value. If it is below that value, then we will change
// to reading from the higher gain circuit.
uint8_t currentPin = CurrentPinSelect(PC3_IsenLG);
// Calculate current and voltage RMS
float currentRMS = RMSval(currentPin,2);
float voltageRMS = RMSval(PC1_Vsen,1);
//
//for(int i=0; i< 10; i++){
//}
float powerFactor = cos(SamplePhaseShift());
// Calculating the power value using all the above values
// and then converting it into W.
float power = (currentRMS * voltageRMS * powerFactor);
// Take this power value, and then give it to the energy
// calculate function to then set the value globally,
// where then the value is written into eeprom.
energy = EnergyCalc(power);
// Send the calculated power for 2 seconds.
setTimerDelay(2);
PORTB &= ~(1<<5);
while(1)
{
// If the overflow is raised, then the 2 seconds
// count must be over, so break out of the loop.
if(TIFR1 & (1<<TOV1))
{
TIFR1 |= (1<<TOV1);
break;
}
// Keep transmitting if 2 seconds aren't over yet
MakeArray_Trans(power);
}
}
//print new line
void PrintLine()
{
UART_Transmit(13);
UART_Transmit(10);
}
