bool ArTCMCompassRobot::packetHandler(ArRobotPacket *packet) { if (packet->getID() != 0xC0) return false; myHeading = ArMath::fixAngle(packet->bufToByte2() / 10.0); myPitch = ArMath::fixAngle(packet->bufToByte2() / 10.0); myRoll = ArMath::fixAngle(packet->bufToByte2() / 10.0); myXMag = packet->bufToByte2() / 100.0; myYMag = packet->bufToByte2() / 100.0; myZMag = packet->bufToByte2() / 100.0; myTemperature = packet->bufToByte2() / 10.0; myError = packet->bufToByte2(); myCalibrationH = packet->bufToByte(); myCalibrationV = packet->bufToByte(); myCalibrationM = packet->bufToByte2() / 100.0; myHaveHeading = myHavePitch = myHaveRoll = myHaveXMag = myHaveYMag = myHaveZMag = myHaveTemperature = myHaveCalibrationH = myHaveCalibrationV = myHaveCalibrationM = true; incrementPacketCount(); invokeHeadingDataCallbacks(myHeading); return true; }
/** * Checks for incoming serial data which will be send by radio. This function * also checks the buffers length to avoid a stackoverflow at the radio FIFO. */ inline void checkTxBuffer(void) { uint8_t i; if(UART0_available() > 0) { for(i=0; i<serialBuffer_write; i++) { if(serialBuffer[i] == '\r' || serialBuffer[i] == '\n' || strlen(data_out_temp) >= 32) { // Transmit data from buffer #ifdef DEBUG if(strlen(data_out_temp) >= MAX_TX_CHARS) printf("max. buffer exceeded\r\n"); #endif // Transmit data incrementPacketCount(); uint8_t n; data_temp[0] = '\0'; n = sprintf(data_temp, "%d%c%s[%s]", NUM_REPEATS, data_count, data_out_temp, NODE_ID); transmitData(n); data_out_temp[0] = '\0'; // Flush buffer } else { sprintf(data_out_temp, "%s%c", data_out_temp, serialBuffer[i]); // Read from serial buffer } } serialBuffer_write = 0; } }
void ArTCMCompassDirect::handleHCHDM(ArNMEAParser::Message m) { myHeading = ArMath::fixAngle(atof((*m.message)[1].c_str())); #ifdef DEBUG_ARTCMCOMPASSDIRECT printf("XXX ArTCMCompassDirect: recieved HCHDM message with compass heading %f.\n", myHeading); #endif myHaveHeading = true; incrementPacketCount(); invokeHeadingDataCallbacks(myHeading); }
int main(void) { // Initialise the GPIO block gpioInit(); #ifdef GPS // Initialise the UART0 block for printf output uart0Init(9600); #else // Initialise the UART0 block for printf output uart0Init(115200); #endif // Configure the multi-rate timer for 1ms ticks mrtInit(__SYSTEM_CLOCK/1000); // Configure the switch matrix (setup pins for UART0 and SPI) configurePins(); //Seed random number generator, we can use our 'unique' ID random_output = NODE_ID[0] + NODE_ID[1] + NODE_ID[2]; //printf("random: %d\r\n", random_output); RFM69_init(); #ifdef GPS int navmode = 9; setupGPS(); #endif #ifdef DEBUG printf("Node initialized, version %s\r\n",GIT_VER); #endif while(1) { #ifdef GPS mrtDelay(5000); navmode = gps_check_nav(); if (navmode != 6){ setupGPS(); } mrtDelay(500); gps_get_position(); mrtDelay(500); gps_check_lock(); mrtDelay(500); //printf("Data: %d,%d,%d,%d,%d,%d\r\n", lat, lon, alt, navmode, lock, sats); //printf("Errors: %d,%d\r\n", GPSerror, serialBuffer_write); #endif incrementPacketCount(); //Clear buffer data_temp[0] = '\0'; uint8_t n; //Create the packet int int_temp = RFM69_readTemp(); // Read transmitter temperature rx_rssi = RFM69_lastRssi(); floor_rssi = RFM69_sampleRssi(); #ifdef GPS n = sprintf(data_temp, "%d%cL%d,%d,%dT%dR%d[%s]", NUM_REPEATS, data_count, lat, lon, alt, int_temp, rx_rssi, NODE_ID); #else if(data_count == 97) { n = sprintf(data_temp, "%d%cL%s[%s]", NUM_REPEATS, data_count, LOCATION_STRING, NODE_ID); } else { n = sprintf(data_temp, "%d%cT%dR%d,%dC%d[%s]", NUM_REPEATS, data_count, int_temp, rx_rssi, floor_rssi, rx_packets, NODE_ID); } #endif transmitData(n); awaitData(TX_GAP); } }
int main(void) { #ifdef ZOMBIE_MODE LPC_SYSCON->BODCTRL = 0x11; //Should be set to Level 1 (Assertion 2.3V, De-assertion 2.4V) reset #endif #if defined(GATEWAY) || defined(DEBUG) // Initialise the UART0 block for printf output uart0Init(115200); #endif // Configure the multi-rate timer for 1ms ticks mrtInit(__SYSTEM_CLOCK/1000); /* Enable AHB clock to the Switch Matrix , UART0 , GPIO , IOCON, MRT , ACMP */ LPC_SYSCON->SYSAHBCLKCTRL |= (1 << 7) | (1 << 14) /*| (1 << 6)*//* | (1 << 18)*/ | (1 << 10) | (1 << 19); // Configure the switch matrix (setup pins for UART0 and SPI) configurePins(); #ifdef DEBUG mrtDelay(100); printf("Node Booted\r\n"); mrtDelay(100); #endif RFM69_init(); #ifdef ZOMBIE_MODE //This is to allow the setup to recover from the initial boot and // avoid a loop RFM69_setMode(RFM69_MODE_SLEEP); init_sleep(); sleepMicro(20000); #endif #ifdef DEBUG printf("Node initialized, version %s\r\n",GIT_VER); #endif //Seed random number generator, we can use our 'unique' ID random_output = NODE_ID[0] + NODE_ID[1] + NODE_ID[2]; while(1) { /* #ifdef ZOMBIE_MODE adc_result = acmpVccEstimate(); // Before transmitting if the input V is too low we could sleep again if (adc_result < 3100 || adc_result > 10000) { sleepRadio(); } #endif */ incrementPacketCount(); //Clear buffer data_temp[0] = '\0'; uint8_t n; //Create the packet int int_temp; #ifdef ZOMBIE_MODE //This is to allow the setup to recover from the initial boot and // avoid a loop RFM69_setMode(RFM69_MODE_SLEEP); init_sleep(); sleepMicro(10000); #endif int_temp = RFM69_readTemp(); // Read transmitter temperature rx_rssi = RFM69_lastRssi(); // read the rssi threshold before re-sampling noise floor which will change it rssi_threshold = RFM69_lastRssiThreshold(); floor_rssi = RFM69_sampleRssi(); #ifdef ZOMBIE_MODE //This is to allow the setup to recover from the initial boot and // avoid a loop RFM69_setMode(RFM69_MODE_SLEEP); init_sleep(); sleepMicro(10000); #endif #ifdef ZOMBIE_MODE adc_result = acmpVccEstimate(); //sleepMicro(20000); #endif #ifdef DEBUG printf("ADC: %d\r\n", adc_result); #endif #ifdef ZOMBIE_MODE //This is to allow the setup to recover from the initial boot and // avoid a loop RFM69_setMode(RFM69_MODE_SLEEP); init_sleep(); sleepMicro(10000); #endif if(data_count == 97) { n = sprintf(data_temp, "%d%cL%s[%s]", NUM_REPEATS, data_count, LOCATION_STRING, NODE_ID); } else { #ifdef DEBUG //n = sprintf(data_temp, "%d%cT%dR%d,%dC%dX%d,%dV%d[%s]", NUM_REPEATS, data_count, int_temp, rx_rssi, floor_rssi, rx_packets, rx_restarts, rssi_threshold, adc_result, NODE_ID); n = sprintf(data_temp, "%d%cT%dV%d[%s]", NUM_REPEATS, data_count, int_temp, adc_result, NODE_ID); #elif defined(ZOMBIE_MODE) n = sprintf(data_temp, "%d%cT%dV%d[%s]", NUM_REPEATS, data_count, int_temp, adc_result, NODE_ID); #else n = sprintf(data_temp, "%d%cT%dR%d[%s]", NUM_REPEATS, data_count, int_temp, rx_rssi, NODE_ID); #endif } transmitData(n); #ifdef ZOMBIE_MODE sleepRadio(); #else awaitData(TX_GAP); #endif } }
int main(void) { #if defined(GATEWAY) || defined(DEBUG) || defined(GPS) // Initialise the UART0 block for printf output uart0Init(115200); #endif // Configure the multi-rate timer for 1ms ticks mrtInit(__SYSTEM_CLOCK/1000); /* Enable AHB clock to the Switch Matrix , UART0 , GPIO , IOCON, MRT , ACMP */ LPC_SYSCON->SYSAHBCLKCTRL |= (1 << 7) | (1 << 14) /*| (1 << 6)*//* | (1 << 18)*/ | (1 << 10) | (1 << 19); // Configure the switch matrix (setup pins for UART0 and SPI) configurePins(); LPC_GPIO_PORT->DIR0 |= (1 << GSM_PWR); RFM69_init(); #if defined(GATEWAY) || defined(DEBUG) || defined(GPS) mrtDelay(100); printf("Node Booted\r\n"); mrtDelay(100); #endif uart1Init(115200); GSM_On(); //GSM_AT(); #if defined(GATEWAY) || defined(DEBUG) || defined(GPS) printf("Node initialized, version %s\r\n",GIT_VER); #endif //Seed random number generator, we can use our 'unique' ID random_output = NODE_ID[0] + NODE_ID[1] + NODE_ID[2]; mrtDelay(5000); GSM_AT(); mrtDelay(5000); GSM_upload(); while(1) { GSM_AT(); incrementPacketCount(); //Clear buffer data_temp[0] = '\0'; uint8_t n; //Create the packet int int_temp; int_temp = RFM69_readTemp(); // Read transmitter temperature rx_rssi = RFM69_lastRssi(); // read the rssi threshold before re-sampling noise floor which will change it rssi_threshold = RFM69_lastRssiThreshold(); floor_rssi = RFM69_sampleRssi(); if(data_count == 97) { n = sprintf(data_temp, "%d%cL%s[%s]", NUM_REPEATS, data_count, LOCATION_STRING, NODE_ID); } else { #ifdef DEBUG n = sprintf(data_temp, "%d%cT%dR%d,%dC%dX%d,%dV%d[%s]", NUM_REPEATS, data_count, int_temp, rx_rssi, floor_rssi, rx_packets, rx_restarts, rssi_threshold, adc_result, NODE_ID); #else n = sprintf(data_temp, "%d%cT%dR%dX%d[%s]", NUM_REPEATS, data_count, int_temp, rx_rssi, rx_packets, NODE_ID); #endif } transmitData(n); GSM_upload(); awaitData(TX_GAP); } }