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;
}
void uart2_print(const char *str){
int i = 0;
while(str[i]!='\0' && i<32){
uart2_putc(str[i]);
i++;
}
}
//=======================================================
void uart2_putROMString(rom char* str)
{
unsigned char c;
while( c = *str++ )
{
while(!TXSTA2bits.TRMT);
uart2_putc(c);
}
}
//=======================================================
void uart2_putString(char *s)
{
char c;
while( (c = *s++) )
{
while(!TXSTA2bits.TRMT);
uart2_putc(c);
}
}
/*!
* Hot restarts the GPS module.
*/
void gps_hot_restart()
{
uart2_puts("$PMTK101*32");
uart2_putc(0x0D); uart2_putc(0x0A);
}
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;
}
PROCESS_THREAD(ble_dfu_process, ev, data){
PROCESS_BEGIN();
/* DFU procedure:
1. Send dfu_reset(0x01) command: 00 01 09 00 01 (no response back)
2. Send dfu_flash_set_address(0x1000) command: 00 04 09 01 00 10 00 00 (check for response)
3. Send dfu_flash_upload([bytes]) command repeatedly (check for response)
- Packet's data payload length byte is 0x41 (65 bytes)
- Each uint8array should have a length byte of 0x40 (64 bytes) followed by 64 bytes of firmware data
- Each response should come back as 00 02 09 02 00 00 if successful
- First packet starts from the :1010... line of the out.hex file, e.g. line 258
4. Send dfu_flash_upload_finish() command: 00 00 09 03 (check for response)
5. Send dfu_reset(0x00) command: 00 01 09 00 00 (no response back)
6. Module will boot back in normal mode, DFU is complete!
*/
const char FILENAME[] = "ble112.bin";
const unsigned int DFU_INITIAL_OFFSET = 4096; // first 4KB is stuff we cannot change
const unsigned int DFU_CHUNK_SIZE = 64;
const unsigned int DFU_ALL_BYTES = 124928;
const char dfu_reset[] = {0x00, 0x01, 0x09, 0x00}; // ad 0x01 to reset into dfu or 0x00 to reset normally
const char dfu_flash_set_address[] = {0x00, 0x04, 0x09, 0x01, 0x00, 0x10, 0x00, 0x00}; // set address to 0x1000
const char dfu_flash_upload[] = {0x00, 0x41, 0x09, 0x02, 0x40}; // followed by 64 bytes of actual data
const char dfu_flash_upload_finish[] = {0x00, 0x00, 0x09, 0x03};
// TODO: maybe do it better
const char rsp_dfu_flash_set_address[] = {0x00, 0x02, 0x09, 0x01, 0x00, 0x00};
const char rsp_dfu_flash_upload[] = {0x00, 0x02, 0x09, 0x02, 0x00, 0x00};
const char rsp_dfu_flash_upload_finish[] = {0x00, 0x02, 0x09, 0x03, 0x00, 0x00};
unsigned int sent;
static char buf[DFU_CHUNK_SIZE + 5];
char i;
// send dfu reset command
for(i=0; i<4; i++)
uart2_putc(dfu_reset[i]);
uart2_putc(0x01);
//should be in dfu now
printf("dfu reset command sent.\n\r");
//wait a sec to make sure ble booted
clock_delay_msec(500); //TODO: figure out smallest reliable
//set flash address
if(ble112_send_command((char*) dfu_flash_set_address, 8, (char*) rsp_dfu_flash_set_address) != 1)
{
// something went wrong.. print debug and terminate
printf("ble112 dfu error: flash set address rsp != 0\n\r");
printf("Update failed. Try again.\n\r");
PROCESS_EXIT();
}
else
printf("flash address set command sent.\n\r");
sent = 0;
while (sent != DFU_ALL_BYTES)
{
// fill the buffer
memcpy(buf, dfu_flash_upload, 5); // copy bgapi flash upload command to the buffer
file_read((char*) FILENAME, &buf[5], (unsigned int) DFU_CHUNK_SIZE, (unsigned int) (DFU_INITIAL_OFFSET + sent));
//send the command and data
if(ble112_send_command(buf, DFU_CHUNK_SIZE + 5, (char*) rsp_dfu_flash_upload) != 1)
{
// something went wrong.. print debug and terminate
printf("ble112 dfu error: flash upload rsp != 0\n\r");
printf("Update failed. Try again.\n\r");
PROCESS_EXIT();
}
sent += DFU_CHUNK_SIZE;
printf("Sent: %u\n\r", sent);
}
// all update data sent here
printf("all update data chunks sent.\n\r");
// send flash upload finish command
if(ble112_send_command((char*) dfu_flash_upload_finish, 4, (char*) rsp_dfu_flash_upload_finish) != 1)
{
// something went wrong.. print debug and terminate
printf("ble112 dfu error: flash upload finish rsp != 0\n\r");
printf("Update failed. Try again.\n\r");
PROCESS_EXIT();
}
else
printf("flash upload finish command sent.\n\r");
// send dfu reset command
for(i=0; i<4; i++)
uart2_putc(dfu_reset[i]);
uart2_putc(0x00);
printf("Update success!\n\r");
PROCESS_END();
}