int ble112_send_command(char* command, char command_len, char* response) { char rsp[6]; char i; for(i=0; i<command_len; i++) { //printf("0x%02x ", command[i]); uart2_putc(command[i]); } //wait until we received a response while (!uart2_can_get()); //read the response back for(i=0; i<6; i++) { rsp[i] = uart2_getc(); //printf("0x%02x ", rsp[i]); } // check response if(memcmp(rsp, response, 6) != 0) { return -1; } else return 1; }
int main (void) { /* variables for the UART0 (USB connection) */ unsigned int c = 0, c2 = 0, c3 = 0; // Variable for reading UARTS char buffer[MAX_MSG_SIZE]; char buffer2[MAX_MSG_SIZE]; char buffer3[MAX_MSG_SIZE]; int idx = 0, idx2 = -1, idx3 = -1; int len2 = 0; int len3 = 0; char meas_buffer[TX_BUFF_SIZE]; int txi = 0; int txtop=0; unsigned int i = 0; char *ptr; unsigned char hli_mutex = 0; unsigned int gps = 0; unsigned int imu = 0; signed int ratio = 0; uint16_t xacc = 0; uint8_t xacca[2]; char s[64]; char rmc[256]; awake_flag = 0; #ifdef RF_TEST_IDX uint8_t gps_rf_test_idx = 0; uint8_t imu_rf_test_idx = 0; #endif /* set outputs */ PORTL = 0xff; // Turn off LEDS DDRL = (1<<LED1) | (1<<LED2) | (1<<LED3) | (1<<LED4); // Set pins for LED as output pwm_init(); spiInit(); /* initialize UARTS */ uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) ); // USB connection uart2_init( UART_BAUD_SELECT(UART2_BAUD_RATE,F_CPU) ); // APC220 radio uart3_init( UART_BAUD_SELECT(UART3_BAUD_RATE,F_CPU) ); // UP-501 GPS /* Interrupt stuff for ADIS */ PCICR |= 1<<PCIE2; // Enable interrupt PORTK PCMSK2 |= (1<<PCINT23); // interrupt in PCINT23 /* now enable interrupt, since UART library is interrupt controlled */ sei(); spiTransferWord(0xBE80); // ADSI software reset _delay_ms(500); /* Set GPS to a faster baud and update UART speed */ //uart3_puts("$PMTK251,115200*1F"); uart3_puts("$PMTK251,57600*2C\r\n"); //uart3_puts("$PMTK251,38400*27"); //uart3_puts("$PMTK251,0*28"); _delay_ms(500); // uart3_init( UART_BAUD_SELECT(115200,F_CPU) ); uart3_init( UART_BAUD_SELECT(57600,F_CPU) ); //uart3_init( UART_BAUD_SELECT(38400,F_CPU) ); /* 115200 seems to be a little bit unstable, at least testing via radio*/ adis_reset_factory(); adis_set_sample_rate(); while (1) { /* Read each UART serially and check each of them for data, if there is handle it */ c = uart_getc(); c2 = uart2_getc(); c3 = uart3_getc(); // Stop motors when connection is lost if (awake_flag > AWAKE_THRESHOLD) { pwm_set_duty(RC1, 0 ); pwm_set_duty(RC2, 0 ); }; if(tx_counter >= TX_READY) { //empty buffer for (txi = 0; txi < txtop; txi++) { uart2_putc(meas_buffer[txi]); // Sending buffered data to RF } txtop = 0; #ifdef AUTO_SHUTDOWN_ENABLE awake_flag++; #endif PORTL ^= (1<<LED2); tx_counter -= TX_READY; } if (adis_ready_counter >= ADIS_READY) { adis_decode_burst_read_pack(&adis_data_decoded); adis_reduce_decoded_burst(); // Reduce data ammount #ifdef LOG_ENABLE hli_send(package(sizeof(adis8_t), 0x14, 0x0D, &adis_data_decoded), sizeof(adis8_t)); // Log to SD card #endif #ifdef RF_TEST_IDX memcpy(&adis_data_decoded_reduced.zgyro[0],&imu_rf_test_idx,1); if (imu_rf_test_idx == 255) imu_rf_test_idx = 1; else imu_rf_test_idx++; memcpy(&meas_buffer[txtop], (char *)package(sizeof(adis8_reduced_t), 0x14, 0x0F, &adis_data_decoded_reduced),sizeof(adis8_reduced_t)+6); #endif #ifndef RF_TEST_IDX memcpy(&meas_buffer[txtop], (char *)package(sizeof(adis8_reduced_t), 0x14, 0x0E, &adis_data_decoded_reduced),sizeof(adis8_reduced_t)+6); #endif txtop=txtop+sizeof(adis8_reduced_t)+6; adis_ready_counter -= ADIS_READY; PORTL ^= (1<<LED4); } /* Reading from radio */ if ( c2 & UART_NO_DATA ) {} else // Data available { //if data is $, set a flag, read next byte, set that value as the length, read while incrementing index until length reached, parse //uart_putc(c2); if (idx2 == 0) { // We should buffer a packet len2 = c2+5; // Set length } if ( (idx2 < len2) && (idx2 >= 0)) { // We are buffering buffer2[idx2] = c2; idx2++; if (idx2 == len2) { // We now have a full packet if (parse(&rfmsg, buffer2)) { PORTL ^= (1<<LED1); process(&rfmsg); } idx2 = -1; // Set flag in new packet mode #ifdef DEBUG //puts_msg(&rfmsg); #endif } } if (c2 == '$') { // We have a possible message comming // PORTL ^= (1<<LED4); idx2 = 0; // Set "flag" } } /* Reading from GPS */ if ( c3 & UART_NO_DATA ) {} else // Data available { /* Transmitting NMEA GPS sentences to the HLI */ if (c3 == '$') { // We have a possible message comming //PORTL ^= (1<<LED3); len3 = 0; // Set "flag" } if (len3 >= 0) { // We are buffering buffer3[len3] = c3; len3++; if (c3 == '\n') { // We now have a full packet if(buffer3[4] != 'S') { // Disable GSV and GSA messages #ifdef LOG_ENABLE hli_send(package(len3, 0x1E, 0x06, buffer3), len3); // Log to SD card #endif if (rmc_cut(buffer3,rmc)) { // Invalid RMC data } else { #ifdef RF_TEST_IDX memcpy(&rmc[0],&gps_rf_test_idx,1); if (gps_rf_test_idx == 255) gps_rf_test_idx = 1; else gps_rf_test_idx++; memcpy(&meas_buffer[txtop], (char *)package(rmc_idx, 30, 31, rmc),rmc_idx+6); #endif #ifndef RF_TEST_IDX memcpy(&meas_buffer[txtop], (char *)package(rmc_idx, 30, 6, rmc),rmc_idx+6); #endif txtop=txtop+rmc_idx+6; PORTL ^= (1<<LED3); } len3 = -1; // Set flag in new packet mode } } } } } return 1; }