bool handle_ICMP_packet(packet_info_t *pi) { debug_println("Packet is ICMP"); byte *icmp_packet = pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH; struct icmp_echo_hdr *icmp_hdr = (void *)pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH; /*unsigned i; for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++) printf("(%d %02X) ", i, (int)*(icmp_packet+i)); printf("\n");*/ if (calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)) { debug_println("ICMP checksum failed, dropping packet!"); return 1; } uint8_t type = ICMPH_TYPE(icmp_hdr); switch (type) { case ICMP_TYPE_ECHO_REQUEST: ICMPH_TYPE_SET(icmp_hdr, 0); break; case ICMP_TYPE_ECHO_REPLY: handle_ping_reply(pi); return 1; default: debug_println("Dropping packet: type %d unknown.", type); return 1; } ICMPH_CHKSUM_SET(icmp_hdr, 0); ICMPH_CHKSUM_SET(icmp_hdr, htons(calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH))); /*for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++) printf("(%d %02X) ", i, (int)*(icmp_packet+i)); printf("\n");*/ return 0; }
void router_init( router_t* router ) { #ifdef _CPUMODE_ init_registers(router); router->nf.device_name = "nf10"; check_iface( &router->nf ); if( openDescriptor( &router->nf ) != 0 ) die( "Error: failed to connect to the hardware" ); else { /* wait for the reset to complete */ struct timespec pause; pause.tv_sec = 0; pause.tv_nsec = 5000 * 1000; /* 5ms */ nanosleep( &pause, NULL ); } #endif router->num_interfaces = 1; router->use_ospf = TRUE; pthread_mutex_init( &router->intf_lock, NULL ); #ifndef _THREAD_PER_PACKET_ debug_println( "Initializing the router work queue with %u worker threads", NUM_WORKER_THREADS ); wq_init( &router->work_queue, NUM_WORKER_THREADS, &router_handle_work ); #else debug_println( "Router initialized (will use one thread per packet)" ); #endif }
void Canvas::fill_row_word_wrapping(std::size_t rw, const std::string& str, std::size_t x_offset, bool centered, bool prefill, char prefill_char, bool postfill, char postfill_char, std::size_t rw_span) { std::stringstream ss_str(str); std::string word; std::string delim(" "); std::size_t line_max_size = cols-x_offset; if (ss_str.good()) { ss_str >> word; debug_println(BIT0,"First word from stringstream: '" << word << "'"); for (std::size_t i = 0 ; i < rw_span; ++i) { std::string line; while(true) { if (ss_str.eof()) { delim.clear(); //If we're at the last word add no delimiter debug_println(BIT0,"End of stringstream reached, delimiter set to blank"); } if (!add_if_fit(word,line,line_max_size,delim)) { if (centered) x_offset = calculate_x_middle(line.size()) + x_offset; //Adjust x_offset for line centration debug_println(BIT0,"Writing line '" << line << "' to canvas matrix"); matrix.fill_row(rw+i,line,x_offset,prefill,prefill_char,postfill,postfill_char); break; } if (ss_str.good()) { ss_str >> word; } else { if (centered) x_offset = calculate_x_middle(line.size()) + x_offset; //Adjust x_offset for line centration matrix.fill_row(rw+i,line,x_offset,prefill,prefill_char,postfill,postfill_char); debug_println(BIT0,"Writing line '" << line << "' to canvas matrix"); goto endwhile; } }
void send_ARP_request(addr_ip_t ip, int num) { char ip_str[16]; ip_to_string(ip_str, ip); debug_println("Sending an ARP request (number %d) to %s:", num, ip_str); byte *packet = malloc_or_die(ARP_PACKET_LENGTH*sizeof(byte)); //Free'd (below). struct arp_hdr *arhdr = (void *)packet; ARH_HARD_TYPE_SET(arhdr, 1); ARH_PROTO_TYPE_SET(arhdr, IPV4_ETHERTYPE); ARH_HARD_LEN_SET(arhdr, 6); ARH_PROTO_LEN_SET(arhdr, 4); ARH_OP_SET(arhdr, 1); interface_t *interface = sr_integ_findsrcintf(ip); if (interface == NULL) { debug_println("ERROR: can't send ARP request, ip %s is not next hop!", ip_str); return; } ARH_SENDER_MAC_SET(arhdr, interface->mac); ARH_SENDER_IP_SET(arhdr, interface->ip); ARH_TARGET_IP_SET(arhdr, ip); ARH_TARGET_MAC_SET(arhdr, make_mac_addr(0, 0, 0, 0, 0, 0)); send_packet(packet, interface->ip, ip, 28, TRUE, FALSE); free(packet); }
//Chris's last minute demo stuff void chris_waypoint_init() { do { gps_update(); } while (gps_get_latitude()==0 || gps_get_longitude()==0); float center_lat = gps_get_latitude(); float center_long = gps_get_longitude(); float circle_scale = 10000; //100 = radius of ~1000m, 1000 = radius of ~100m, 4000 = radius of ~ 25m debug_print("Generating circle of waypoints around center latitude "); debug_printf(center_lat); debug_print(", longitude "); debug_printf(center_lat); debug_println("..."); for(int i = 0; i < NUM_WAYPOINTS; i++) { float temp_lat, temp_long; temp_lat = center_lat-cos((2*PI*i)/NUM_WAYPOINTS)/circle_scale; temp_long = center_long+sin((2*PI*i)/NUM_WAYPOINTS)/circle_scale; chris_waypoint_add(temp_lat,temp_long); debug_printf(temp_lat); debug_print(", "); debug_printf(temp_long); debug_println(""); } chris_array_index=0; }
void exec_periodic_thread_B(void) { debug_println("Executing B (periodic)"); thread_b_read_datap(&B_data); debug_println("Value of B_data after read:"); debug_printhex32(B_data.field); debug_println("task B complete"); }
status_t LSM303C::ACC_Status_Flags(uint8_t& val) { debug_println("Getting accel status"); if( ACC_ReadReg(ACC_STATUS, val) ) { debug_println(AERROR); return IMU_HW_ERROR; } return IMU_SUCCESS; }
void i2cdriver_init(void) { i2cStart(&I2CD1, &i2c_conf); // Start the I2C1 driver debug_println("Init: I2C Driver Active"); accel_init(); // Prepare the accelerometer debug_println("Init: Accelerometer Active"); chThdSleepMilliseconds(100); // May not be needed. Using as in example code. gyro_init(); // Prepare the gyroscope debug_println("Init: Gyroscope Active"); chThdSleepMilliseconds(100); // Also may not be needed. }
void exec_periodic_thread_A(void) { debug_println("Executing A (periodic)"); if (A_data.field < 10) { A_data.field ++ ; thread_a_write_foo_data(&A_data); } else { } debug_println("task A complete"); }
bool initialise() { bool success = true; USART_init(USART_PC,9600); debug_println("Begininning Initialisation..."); initTimers(); if(gps_demonstration==true) { //GPS Demonstration altimeter_init(); init_HMC5883L(); gps_init(); //Initialise a series of waypoints in a circle around the current coordinates.. _delay_ms(1000); chris_waypoint_init(); } else { //RX and servo Demonstration rx_init(); quad_output_init(); } debug_println("Initialization succeeded!"); //beep some pattern I can recognize debug_beep_long(); _delay_ms(250); debug_beep(); _delay_ms(250); debug_beep(); _delay_ms(250); debug_beep(); _delay_ms(250); debug_beep_long(); return success; }
ancs_notification_t* ancs_notification_cached() { static ancs_notification_t notification_cache; debug_print(F("ancs_notification_cached(")); debug_print(notification_cache.uid); debug_println(F(")")); return ¬ification_cache; }
/* Handler for tick interrupts. * This function is of return type void because we designated it non-preempting in the system XML. * This is an optimization appropriate for interrupt handlers that don't raise any interrupt events or otherwise take * any actions that may affect the schedulability of any tasks. */ void tick_interrupt(void) { machine_timer_clear(); debug_println("tick_interrupt"); }
void ping_received1(const uint32_t *periodic_100_ms) { debug_print("receiver1 ping received"); debug_printhex32(*periodic_100_ms); debug_println(""); uint32_t newValue = 2*(*periodic_100_ms); ping_Output2(&newValue); }
bool ancs_send_buffered_command() { if (!command_send_enable && ((millis() - last_command_send) < 1000)) { return true; } uint8_t* buffer; size_t len = fifo_pop(buffer_commands, &buffer); if (len == 0) { return false; } uint8_t cmd, aid; uint32_t uid; unpack(buffer, "BIB", &cmd, &uid, &aid); debug_print(F("[ANCS CP] Command sent 0x")); debug_print(cmd, HEX); debug_print(F(" for notification #")); debug_print(uid, DEC); debug_print(F(" attribute 0x")); debug_print(aid, HEX); debug_println(); if (lib_aci_send_data(PIPE_ANCS_CONTROL_POINT_TX_ACK, buffer, len)) { command_send_enable = false; last_command_send = millis(); } else { Serial.print(F("!! Error sending data for notification #")); Serial.println(uid, DEC); } free(buffer); return true; }
void router_add_arp_entry( router_t *router, addr_mac_t mac, addr_ip_t ip, bool dynamic) { ip_mac_t *arp_entry; pthread_mutex_lock(&router->arp_cache_lock); debug_println("Adding arp entry."); // TODO remove debugging line arp_entry = &router->arp_cache[router->num_arp_cache]; arp_entry->mac = mac; arp_entry->ip = ip; arp_entry->time = get_time(); arp_entry->dynamic = dynamic; router->num_arp_cache += 1; #ifdef _CPUMODE_ writeReg(router->nf.fd, XPAR_NF10_ROUTER_OUTPUT_PORT_LOOKUP_0_ARP_IP, ntohl(ip)); writeReg(router->nf.fd, XPAR_NF10_ROUTER_OUTPUT_PORT_LOOKUP_0_ARP_MAC_LOW, mac_lo(&mac)); writeReg(router->nf.fd, XPAR_NF10_ROUTER_OUTPUT_PORT_LOOKUP_0_ARP_MAC_HIGH, mac_hi(&mac)); writeReg(router->nf.fd, XPAR_NF10_ROUTER_OUTPUT_PORT_LOOKUP_0_ARP_WR_ADDR, router->num_arp_cache-1); #endif pthread_mutex_unlock(&router->arp_cache_lock); }
// Public methods status_t LSM303C::begin() { debug_println(EMPTY); return begin(// Default to I2C bus MODE_I2C, // Initialize magnetometer output data rate to 0.625 Hz (turn on device) MAG_DO_40_Hz, // Initialize magnetic field full scale to +/-16 gauss MAG_FS_16_Ga, // Enabling block data updating MAG_BDU_ENABLE, // Initialize magnetometer X/Y axes ouput data rate to high-perf mode MAG_OMXY_HIGH_PERFORMANCE, // Initialize magnetometer Z axis performance mode MAG_OMZ_HIGH_PERFORMANCE, // Initialize magnetometer run mode. Also enables I2C (bit 7 = 0) MAG_MD_CONTINUOUS, // Initialize acceleration full scale to +/-2g ACC_FS_2g, // Enable block data updating ACC_BDU_ENABLE, // Enable X, Y, and Z accelerometer axes ACC_X_ENABLE|ACC_Y_ENABLE|ACC_Z_ENABLE, // Initialize accelerometer output data rate to 100 Hz (turn on device) ACC_ODR_100_Hz ); }
void t0(void) { debug_println("t0"); for (;;) { } }
int main(void) { initialise(); uint16_t last_changed = millis(); while(1) { if(gps_demonstration==true) { if(gps_update()) { chris_waypoint_update(); debug_print("H"); debug_print(","); debug_printf(gps_get_latitude()); debug_print(","); debug_printf(gps_get_longitude()); debug_print(","); debug_printi(gps_get_sats()); debug_print(","); debug_printf(altimeter_get_metres()); debug_print(","); debug_printf(chris_waypoint_current_bearing()); //target heading debug_print(","); float current_heading = compass_get_heading(); debug_printf(current_heading); debug_print(","); debug_printf(current_heading-chris_waypoint_current_bearing()); debug_print(","); debug_printi(chris_waypoint_current_index()); debug_print(","); debug_printf(chris_waypoint_current_distance()); debug_print(","); debug_printf(array_get_lat(chris_waypoint_current_index())); debug_print(","); debug_printf(array_get_lon(chris_waypoint_current_index())); debug_println(","); } //automatic testing, every 3 seconds change to the next waypoint if(millis()-last_changed>3000 && 1==2) { chris_set_waypoint_current_index(chris_waypoint_current_index()+1); last_changed=millis(); } if(chris_waypoint_current_index()>chris_get_max_waypoints()) chris_set_waypoint_current_index(0); } else { //demonstrate reading receiver rx_update(); quad_output_passthrough(true,true,true,true,true,true); } } }
void generate_pending_ARP_thread() { router_t *router = get_router(); pending_arp_entry_t expiring_arp_entry[router->num_pending_arp]; unsigned i; while (TRUE) { unsigned num_expiring = 0; pthread_mutex_lock(&router->pending_arp_lock); for (i = 0; i < router->num_pending_arp; i++) { pending_arp_entry_t *pending_arp_entry = &router->pending_arp[i]; if (pending_arp_entry->num_sent >= 5) { debug_println("Not responding to ARP request!"); expiring_arp_entry[num_expiring].payload = pending_arp_entry->payload; expiring_arp_entry[num_expiring].len = pending_arp_entry->len; num_expiring++; unsigned j; for (j = i; j < router->num_pending_arp-1; j++) { router->pending_arp[j] = router->pending_arp[j+1]; } router->num_pending_arp--; if (i != router->num_pending_arp) i--; //Don't increase i on next go. continue; } send_ARP_request(pending_arp_entry->ip, ++pending_arp_entry->num_sent); //Shouldn't need lock. } debug_println("num_pending_arp=%d", router->num_pending_arp); pthread_mutex_unlock(&router->pending_arp_lock); for (i = 0; i < num_expiring; i++) { struct ip_hdr *iphdr = (void *)expiring_arp_entry[i].payload; if (router_lookup_interface_via_ip(router, IPH_SRC(iphdr)) == NULL) { //Don't send a response to itself. handle_not_repsponding_to_arp(expiring_arp_entry[i].payload, expiring_arp_entry[i].len); } else { debug_println("NOT sending response, because it originated from own interface."); } free(expiring_arp_entry[i].payload); expiring_arp_entry[i].payload = NULL; } sleep(1); debug_println("Pending ARP thread sleeping for 1s."); } }
void handle_not_repsponding_to_arp(byte *payload, unsigned len) { debug_println("Not responding to arp:\n"); packet_info_t *pi = malloc_or_die(sizeof(packet_info_t)); //Free'd (below). pi->packet = malloc_or_die((IPV4_HEADER_OFFSET+len)*sizeof(uint8_t)); //Free'd (below). pi->len = len; memcpy(pi->packet+IPV4_HEADER_OFFSET, payload, len); struct eth_hdr *ethhdr = (void *)pi->packet; ETH_DEST_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0)); ETH_SRC_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0)); ETH_TYPE_SET(ethhdr, 0); //Reverse soruce and destination again. struct ip_hdr *iphdr;/* = (void *)pi->packet+IPV4_HEADER_OFFSET; swap_bytes(&IPH_SRC(iphdr), &IPH_DEST(iphdr), 4);*/ unsigned i; for (i = 0; i < pi->len; i += 2) printf("%02X%02X ", *(pi->packet+i),*(pi->packet+i+1)); printf("\n"); if (generate_response_ICMP_packet(pi, 3, 1)) return; iphdr = (void *)pi->packet+IPV4_HEADER_OFFSET; //pi->packet have moved in memory, so re-define. IPH_CHKSUM_SET(iphdr, 0); IPH_CHKSUM_SET(iphdr, htons(calc_checksum(pi->packet+IPV4_HEADER_OFFSET, IPV4_HEADER_LENGTH))); for (i = 0; i < pi->len; i += 2) printf("%02X%02X ", *(pi->packet+i),*(pi->packet+i+1)); printf("\n"); addr_ip_t target_ip = sr_integ_findnextip(IPH_DEST(iphdr)); char target_ip_str[STRLEN_IP]; ip_to_string(target_ip_str, target_ip); debug_println("target_ip=%s", target_ip_str); //send_packet(pi->packet+IPV4_HEADER_OFFSET, IPH_SRC(iphdr), target_ip, pi->len-IPV4_HEADER_OFFSET, FALSE, FALSE); free(pi->packet); free(pi); }
// Decodes the received IR message // Returns 0 if no data ready, 1 if data ready. // Results of decoding are stored in results int IRrecv::decode(decode_results *results) { if (decodeStart(results) == ERR) return ERR; debug_println("Attempting NEC decode"); if (decodeNEC(results)) { return DECODED; } debug_println("Attempting Sony decode"); if (decodeSony(results)) { return DECODED; } debug_println("Attempting Sanyo decode"); if (decodeSanyo(results)) { return DECODED; } debug_println("Attempting Mitsubishi decode"); if (decodeMitsubishi(results)) { return DECODED; } debug_println("Attempting RC5 decode"); if (decodeRC5(results)) { return DECODED; } debug_println("Attempting RC6 decode"); if (decodeRC6(results)) { return DECODED; } debug_println("Attempting Panasonic decode"); if (decodePanasonic(results)) { return DECODED; } debug_println("Attempting JVC decode"); if (decodeJVC(results)) { return DECODED; } // decodeHash returns a hash on any input. // Thus, it needs to be last in the list. // If you add any decodes, add them before this. if (decodeHash(results)) { return DECODED; } // Throw away and start over resume(); return ERR; }
status_t LSM303C::begin(InterfaceMode_t im, MAG_DO_t modr, MAG_FS_t mfs, MAG_BDU_t mbu, MAG_OMXY_t mxyodr, MAG_OMZ_t mzodr, MAG_MD_t mm, ACC_FS_t afs, ACC_BDU_t abu, uint8_t aea, ACC_ODR_t aodr) { uint8_t successes = 0; // Select I2C or SPI interfaceMode = im; if (interfaceMode == MODE_SPI) { debug_println("Setting up SPI"); // Setup pins for SPI // CS & CLK must be outputs DDRxn = 1 bitSet(DIR_REG, CSBIT_MAG); bitSet(DIR_REG, CSBIT_XL); bitSet(DIR_REG, CLKBIT); // Deselect SPI chips bitSet(CSPORT_MAG, CSBIT_MAG); bitSet(CSPORT_XL, CSBIT_XL); // Clock polarity (CPOL) = 1 bitSet(CLKPORT, CLKBIT); // SPI Serial Interface Mode (SIM) bits must be set SPI_WriteByte(ACC, ACC_CTRL4, 0b111); SPI_WriteByte(MAG, MAG_CTRL_REG3, _BV(2)); } else { Wire.begin(); Wire.setClock(400000L); } ////////// Initialize Magnetometer ////////// // Initialize magnetometer output data rate successes += MAG_SetODR(modr); // Initialize magnetic field full scale successes += MAG_SetFullScale(mfs); // Enabling block data updating successes += MAG_BlockDataUpdate(mbu); // Initialize magnetometer X/Y axes ouput data rate successes += MAG_XY_AxOperativeMode(mxyodr); // Initialize magnetometer Z axis performance mode successes += MAG_Z_AxOperativeMode(mzodr); // Initialize magnetometer run mode. successes += MAG_SetMode(mm); ////////// Initialize Accelerometer ////////// // Initialize acceleration full scale successes += ACC_SetFullScale(afs); // Enable block data updating successes += ACC_BlockDataUpdate(abu); // Enable X, Y, and Z accelerometer axes successes += ACC_EnableAxis(aea); // Initialize accelerometer output data rate successes += ACC_SetODR(aodr); return (successes == IMU_SUCCESS) ? IMU_SUCCESS : IMU_HW_ERROR; }
void printwaypoint(WayPoint wps[], int nwps) { int i; int j = 1; i = nwps; char *ptr_1 = "Summary"; char *ptr_2 = ("========================="); char *ptr_3 = ("End Summary"); char *ptr_4 = " "; USART_putstring(USART_PC,"\n\r"); debug_println(ptr_1); debug_println(ptr_2); for (i = 0; i < nwps; i++) { debug_printf(wps[i].latitude);debug_print(ptr_4);/*debug_printf(wps[i].latitude_dist);debug_print(ptr_4);*/debug_printf(wps[i].latitude_deg);debug_print(ptr_4);debug_printf(wps[i].longitude);/*debug_print(ptr_4);debug_printf(wps[i].longitude_dist);*/debug_print(ptr_4);debug_printf(wps[i].longitude_deg); USART_putstring(USART_PC,"\n\r"); j = j + 1; } debug_println(ptr_2); debug_println(ptr_3); USART_putstring(USART_PC,"\n\r"); }
/* Fatal error function provided for debugging purposes. */ void fatal(const RtosErrorId error_id) { debug_print("FATAL ERROR: "); debug_printhex32(error_id); debug_println(""); /* Disable interrupts */ asm volatile("wrteei 0"); for (;;) ; }
void fatal(const RtosErrorId error_id) { debug_print("FATAL ERROR: "); debug_printhex32(error_id); debug_println(""); for (;;) { } }
Item::Item(std::string filename) { std::ifstream fs_item (filename); if (!fs_item.good()) { throw FileException("File '" + filename + "' not found or is empty"); } std::getline(fs_item, name); std::getline(fs_item, description); debug_println(BIT6,"Item created from file '" << filename << "' has name set to " << name << " and description set to " << description); }
void* router_pthread_main( void* vpacket ) { static unsigned id = 0; char name[15]; snprintf( name, 15, "PHandler %u", id++ ); debug_pthread_init( name, "Packet Handler Thread" ); pthread_detach( pthread_self() ); router_handle_packet( (packet_info_t*)vpacket ); debug_println( "Packet Handler Thread is shutting down" ); return NULL; }
status_t LSM303C::I2C_ByteRead(I2C_ADDR_t slaveAddress, uint8_t reg, uint8_t& data) { status_t ret = IMU_GENERIC_ERROR; debug_print("Reading from I2C address: 0x"); debug_prints(slaveAddress, HEX); debug_prints(", register 0x"); debug_printlns(reg, HEX); Wire.beginTransmission(slaveAddress); // Initialize the Tx buffer if (Wire.write(reg)) // Put slave register address in Tx buff { if (Wire.endTransmission(false)) // Send Tx, send restart to keep alive { debug_println("Error: I2C buffer didn't get sent!"); debug_print("Slave address: 0x"); debug_printlns(slaveAddress, HEX); debug_print("Register: 0x"); debug_printlns(reg, HEX); ret = IMU_HW_ERROR; } else if (Wire.requestFrom(slaveAddress, 1)) { data = Wire.read(); debug_print("Read: 0x"); debug_printlns(data, HEX); ret = IMU_SUCCESS; } else { debug_println("IMU_HW_ERROR"); ret = IMU_HW_ERROR; } } else { debug_println("Error: couldn't send slave register address"); } return ret; }
void* eChronosCreateMutex(void) { if(firstcall_init_need){ xSemMux_init(); firstcall_init_need = 0; } void * priv = alloc_xSemMux(BINARY_MUTEX, 0, 0); if(priv == NULL){ #ifdef ECHRONOS_DEBUG_ENABLE debug_println("error 3\n"); #endif return NULL; } int i = 0; for(i = 0; i < xSEM_MUX_NUM_MAX; i++){ if(xSemMuxList[i].created == 0){ xSemMuxList[i].priv = priv; xSemMuxList[i].type = BINARY_MUTEX; xSemMuxList[i].created = 1; break; } } if(i >= xSEM_MUX_NUM_MAX){ #ifdef ECHRONOS_DEBUG_ENABLE debug_println("error 4\n"); #endif return NULL; } return (void *)&xSemMuxList[i]; }
float LSM303C::readAccel(AXIS_t dir) { uint8_t flag_ACC_STATUS_FLAGS; status_t response = ACC_Status_Flags(flag_ACC_STATUS_FLAGS); if (response != IMU_SUCCESS) { debug_println(AERROR); return NAN; } // Check for new data in the status flags with a mask // If there isn't new data use the last data read. // There are valid cases for this, like reading faster than refresh rate. if (flag_ACC_STATUS_FLAGS & ACC_ZYX_NEW_DATA_AVAILABLE) { response = ACC_GetAccRaw(accelData); debug_println("Fresh raw data"); } //convert from LSB to mg switch (dir) { case xAxis: return accelData.xAxis * SENSITIVITY_ACC; break; case yAxis: return accelData.yAxis * SENSITIVITY_ACC; break; case zAxis: return accelData.zAxis * SENSITIVITY_ACC; break; default: return NAN; } // Should never get here debug_println("Returning NAN"); return NAN; }