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); }