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