static void _api_at_cmd(xbee_t *dev, uint8_t *cmd, uint8_t size, resp_t *resp)
{
DEBUG("[xbee] AT_CMD: %s\n", cmd);
/* acquire TX lock */
mutex_lock(&(dev->tx_lock));
/* construct API frame */
dev->cmd_buf[0] = API_START_DELIMITER;
dev->cmd_buf[1] = (size + 2) >> 8;
dev->cmd_buf[2] = (size + 2) & 0xff;
dev->cmd_buf[3] = API_ID_AT;
dev->cmd_buf[4] = 1; /* use fixed frame id */
memcpy(dev->cmd_buf + 5, cmd, size);
dev->cmd_buf[size + 5] = _cksum(3, dev->cmd_buf, size + 5);
/* reset the response data counter */
dev->resp_count = 0;
/* start send data */
uart_write(dev->p.uart, dev->cmd_buf, size + 6);
xtimer_ticks64_t sent_time = xtimer_now64();
xtimer_t resp_timer;
resp_timer.callback = isr_resp_timeout;
resp_timer.arg = dev;
xtimer_set(&resp_timer, RESP_TIMEOUT_USEC);
/* wait for results */
while ((dev->resp_limit != dev->resp_count) &&
(xtimer_less(
xtimer_diff32_64(xtimer_now64(), sent_time),
xtimer_ticks_from_usec(RESP_TIMEOUT_USEC)))) {
mutex_lock(&(dev->resp_lock));
}
xtimer_remove(&resp_timer);
if (dev->resp_limit != dev->resp_count) {
DEBUG("[xbee] api_at_cmd: response timeout\n");
resp->status = 255;
mutex_unlock(&(dev->tx_lock));
return;
}
/* populate response data structure */
resp->status = dev->resp_buf[3];
resp->data_len = dev->resp_limit - 5;
if (resp->data_len > 0) {
memcpy(resp->data, &(dev->resp_buf[4]), resp->data_len);
}
mutex_unlock(&(dev->tx_lock));
}
static void test_fib_15_get_lifetime(void)
{
uint64_t lifetime, now;
kernel_pid_t iface_id = 1;
char addr_dst[] = "Test address151";
char addr_nxt[] = "Test address152";
size_t add_buf_size = 16;
uint32_t addr_dst_flags = 0x77777777;
uint32_t addr_nxt_flags = 0x77777777;
TEST_ASSERT_EQUAL_INT(0, fib_add_entry(&test_fib_table,
iface_id, (uint8_t *)addr_dst, add_buf_size - 1,
addr_dst_flags, (uint8_t *)addr_nxt, add_buf_size - 1,
addr_nxt_flags, 1000));
TEST_ASSERT_EQUAL_INT(0, fib_devel_get_lifetime(&test_fib_table, &lifetime,
(uint8_t *)addr_dst,
add_buf_size - 1));
/* assuming some ms passed during these operations... */
now = xtimer_now64();
uint64_t cmp_lifetime = now + 900000lU;
uint64_t cmp_max_lifetime = now + 1100000lU;
TEST_ASSERT_EQUAL_INT(1, (lifetime > cmp_lifetime));
/* make sure lifetime hasn't grown magically either */
TEST_ASSERT_EQUAL_INT(1, (lifetime < cmp_max_lifetime));
fib_deinit(&test_fib_table);
}
int udp_send(int argc, char **argv)
{
if (argc != 4) {
puts("Usage: udp <ipv6-addr> <port> <payload>");
return -1;
}
int res;
ipv6_addr_t src = IPV6_ADDR_UNSPECIFIED, dst;
if (ipv6_addr_from_str(&dst, argv[1]) == NULL) {
puts("Error: unable to parse destination address");
return 1;
}
if((res = conn_udp_sendto(argv[3], strlen(argv[3]), &src, sizeof(src), &dst, sizeof(dst),
AF_INET6, 1234, (uint16_t) (atoi(argv[2])))) < 0) {
puts("could not send");
}
else {
//printf("Success: send %u byte to %s\n", (unsigned) res, argv[1]);
if (gnrc_rpl_instances[0].state && gnrc_rpl_instances[0].dodag.node_status != GNRC_RPL_ROOT_NODE) {
if (acked) {
time = xtimer_now64();
acked = false;
}
}
}
return 0;
}
u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t count)
{
LWIP_ASSERT("invalid semaphor", sys_sem_valid(sem));
if (count != 0) {
uint64_t stop, start;
start = xtimer_now64();
int res = sema_wait_timed((sema_t *)sem, count * MS_IN_USEC);
stop = xtimer_now64() - start;
if (res == -ETIMEDOUT) {
return SYS_ARCH_TIMEOUT;
}
return (u32_t)(stop / MS_IN_USEC);
}
else {
sema_wait_timed((sema_t *)sem, 0);
return 0;
}
}
void *_udp_server(void *args)
{
uint16_t port = (uint16_t) atoi(args);
ipv6_addr_t server_addr = IPV6_ADDR_UNSPECIFIED;
msg_init_queue(server_msg_queue, SERVER_MSG_QUEUE_SIZE);
if(conn_udp_create(&conn, &server_addr, sizeof(server_addr), AF_INET6, port) < 0) {
return NULL;
}
server_running = true;
printf("Success: started UDP server on port %" PRIu16 "\n", port);
char *arg[4];
char *cmd = "udp_send";
char *port_str = "8888";
arg[0] = cmd;
arg[2] = port_str;
char src_str[IPV6_ADDR_MAX_STR_LEN];
while (1) {
int res;
ipv6_addr_t src;
size_t src_len = sizeof(ipv6_addr_t);
if ((res = conn_udp_recvfrom(&conn, server_buffer, sizeof(server_buffer),
&src, &src_len, &port)) < 0) {
puts("Error while receiving");
}
else if (res == 0) {
puts("No data received");
}
else {
server_buffer[res] = '\0';
if (gnrc_rpl_instances[0].state && gnrc_rpl_instances[0].dodag.node_status == GNRC_RPL_ROOT_NODE) {
printf("%s;%s\n", ipv6_addr_to_str(src_str, &src, sizeof(src_str)),
server_buffer);
ipv6_addr_to_str(addr_str, &ipv6_addr_all_nodes_link_local, sizeof(addr_str));
arg[1] = addr_str;
arg[3] = src_str;
udp_send(4, arg);
}
else {
ipv6_addr_t payload;
ipv6_addr_from_str(&payload, server_buffer);
if ((gnrc_ipv6_netif_find_by_addr(NULL, &payload) != KERNEL_PID_UNDEF) && (!acked)) {
acked = true;
printf("diff: %llu\n", xtimer_now64() - time);
}
}
}
}
return NULL;
}
bool initialiseIMU(mpu9250_t *dev)
{
int result;
printf("+------------Initializing------------+\n");
result = mpu9250_init(dev, I2C_0, MPU9250_HW_ADDR_HEX_68, MPU9250_COMP_ADDR_HEX_0C);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] The compass did not answer correctly on the given address");
return false;
}
result = mpu9250_set_gyro_fsr(dev, GFSR);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] Invalid Gyro FSR value");
return false;
}
result = mpu9250_set_accel_fsr(dev, AFSR);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return false;
}
else if (result == -2) {
puts("[Error] Invalid Accel FSR value");
return false;
}
mpu9250_set_sample_rate(dev, GA_SAMPLE_RATE_HZ);
uint16_t gaSampleRate = dev->conf.sample_rate;
printf("G+A sample rate set to: %u (requested rate was %u)\n", gaSampleRate, GA_SAMPLE_RATE_HZ);
mpu9250_set_compass_sample_rate(dev, C_SAMPLE_RATE_HZ);
uint16_t cSampleRate = dev->conf.compass_sample_rate;
printf("Compass sample rate set to: %u (requested rate was %u)\n", cSampleRate, C_SAMPLE_RATE_HZ);
printf("Initialization successful\n\n");
t0 = xtimer_now64();
autoCalibrate = true;
dupInterval = UPDATE_INTERVAL_MS;
initialisePosition();
return true;
}
int main(void)
{
uint32_t n = ITERATIONS;
uint64_t now;
uint64_t before = xtimer_now64();
while(--n) {
now = xtimer_now64();
if ((now-before) > MAXDIFF) {
puts("TEST FAILED.");
break;
}
before = now;
}
if (!n) {
puts("TEST SUCCESSFUL.");
}
return 0;
}
/**
* @brief Make a raw dump of the given packet contents
*/
void dump_pkt(gnrc_pktsnip_t *pkt)
{
gnrc_pktsnip_t *snip = pkt;
printf("rftest-rx --- len 0x%02x lqi 0x%02x rx_time 0x%08" PRIx64 "\n\n",
gnrc_pkt_len(pkt), 0, xtimer_now64());
while (snip) {
for (size_t i = 0; i < snip->size; i++) {
printf("0x%02x ", ((uint8_t *)(snip->data))[i]);
}
snip = snip->next;
}
puts("\n");
gnrc_pktbuf_release(pkt);
}
int sntp_sync(sock_udp_ep_t *server, uint32_t timeout)
{
int result;
mutex_lock(&_sntp_mutex);
if ((result = sock_udp_create(&_sntp_sock,
NULL,
server,
0)) < 0) {
DEBUG("Error creating UDP sock\n");
mutex_unlock(&_sntp_mutex);
return result;
}
memset(&_sntp_packet, 0, sizeof(_sntp_packet));
ntp_packet_set_vn(&_sntp_packet);
ntp_packet_set_mode(&_sntp_packet, NTP_MODE_CLIENT);
if ((result = (int)sock_udp_send(&_sntp_sock,
&_sntp_packet,
sizeof(_sntp_packet),
NULL)) < 0) {
DEBUG("Error sending message\n");
sock_udp_close(&_sntp_sock);
mutex_unlock(&_sntp_mutex);
return result;
}
if ((result = (int)sock_udp_recv(&_sntp_sock,
&_sntp_packet,
sizeof(_sntp_packet),
timeout,
NULL)) < 0) {
DEBUG("Error receiving message\n");
sock_udp_close(&_sntp_sock);
mutex_unlock(&_sntp_mutex);
return result;
}
sock_udp_close(&_sntp_sock);
_sntp_offset = (byteorder_ntohl(_sntp_packet.transmit.seconds) * US_PER_SEC) +
((byteorder_ntohl(_sntp_packet.transmit.fraction) * 232)
/ 1000000) - xtimer_now64();
mutex_unlock(&_sntp_mutex);
return 0;
}
void _update_lifetime(void)
{
uint64_t now = xtimer_now64();
gnrc_rpl_parent_t *parent;
for (uint8_t i = 0; i < GNRC_RPL_PARENTS_NUMOF; ++i) {
parent = &gnrc_rpl_parents[i];
if (parent->state != 0) {
if ((int64_t)(parent->lifetime - now) <= (int64_t) (GNRC_RPL_LIFETIME_UPDATE_STEP
* SEC_IN_USEC)) {
gnrc_rpl_dodag_t *dodag = parent->dodag;
gnrc_rpl_parent_remove(parent);
gnrc_rpl_parent_update(dodag, NULL);
continue;
}
else if ((int64_t)(parent->lifetime - now) <=
(int64_t) (GNRC_RPL_LIFETIME_UPDATE_STEP * SEC_IN_USEC * 2)) {
gnrc_rpl_send_DIS(parent->dodag, &parent->addr);
}
}
}
xtimer_set_msg(&_lt_timer, _lt_time, &_lt_msg, gnrc_rpl_pid);
}
bool getIMUData(mpu9250_t dev, imuData_t *data)
{
data->ts = xtimer_now64() - t0;
if (mpu9250_read_accel(&dev, &data->accel))
{
return false;
}
if (mpu9250_read_gyro(&dev, &data->gyro))
{
return false;
}
if (mpu9250_read_compass(&dev, &data->mag))
{
return false;
}
imuCalibrate(data);
int32_t rawTemp;
mpu9250_read_temperature(&dev, &rawTemp);
data->temperature = rawTemp/1000; // approx temperature in degrees C
return true;
}
void fib_print_routes(fib_table_t *table)
{
mutex_lock(&mtx_access);
printf("%-" FIB_ADDR_PRINT_LENS "s %-6s %-" FIB_ADDR_PRINT_LENS "s %-6s %-16s Interface\n"
, "Destination", "Flags", "Next Hop", "Flags", "Expires");
uint64_t now = xtimer_now64();
for (size_t i = 0; i < table->size; ++i) {
if (table->entries[i].lifetime != 0) {
fib_print_address(table->entries[i].global);
printf(" 0x%04"PRIx32" ", table->entries[i].global_flags);
fib_print_address(table->entries[i].next_hop);
printf(" 0x%04"PRIx32" ", table->entries[i].next_hop_flags);
if (table->entries[i].lifetime != FIB_LIFETIME_NO_EXPIRE) {
uint64_t tm = table->entries[i].lifetime - now;
/* we must interpret the values as signed */
if ((int64_t)tm < 0 ) {
printf("%-16s ", "EXPIRED");
}
else {
printf("%"PRIu32".%05"PRIu32, (uint32_t)(tm / 1000000), (uint32_t)(tm % 1000000));
}
}
else {
printf("%-16s ", "NEVER");
}
printf("%d\n", (int)table->entries[i].iface_id);
}
}
mutex_unlock(&mtx_access);
}
}
/**
* @brief Send a signal to a thread
*
* @param[in] pid the pid to send to
* @param[in] sig the signal to send
*
* @return TODO
*/
__attribute__ ((weak))
int _kill(pid_t pid, int sig)
{
(void) pid;
(void) sig;
errno = ESRCH; /* not implemented yet */
return -1;
}
#ifdef MODULE_XTIMER
int _gettimeofday_r(struct _reent *r, struct timeval *restrict tp, void *restrict tzp)
{
(void)tzp;
(void) r;
uint64_t now = xtimer_now64();
tp->tv_sec = div_u64_by_1000000(now);
tp->tv_usec = now - (tp->tv_sec * SEC_IN_USEC);
return 0;
}
#endif
/**
* @brief convert an offset given in ms to abolute time in time in us
* @param[in] ms the milliseconds to be converted
* @param[out] target the converted point in time
*/
static void fib_lifetime_to_absolute(uint32_t ms, uint64_t *target)
{
*target = xtimer_now64() + (ms * 1000);
}
/**
* @brief returns pointer to the entry for the given destination address
*
* @param[in] table the FIB table to search in
* @param[in] dst the destination address
* @param[in] dst_size the destination address size
* @param[out] entry_arr the array to scribe the found match
* @param[in, out] entry_arr_size the number of entries provided by entry_arr (should be always 1)
* this value is overwritten with the actual found number
*
* @return 0 if we found a next-hop prefix
* 1 if we found the exact address next-hop
* -EHOSTUNREACH if no fitting next-hop is available
*/
static int fib_find_entry(fib_table_t *table, uint8_t *dst, size_t dst_size,
fib_entry_t **entry_arr, size_t *entry_arr_size) {
uint64_t now = xtimer_now64();
size_t count = 0;
size_t prefix_size = 0;
size_t match_size = dst_size<<3;
int ret = -EHOSTUNREACH;
bool is_all_zeros_addr = true;
for(size_t i = 0; i < dst_size; ++i) {
if (dst[i] != 0) {
is_all_zeros_addr = false;
break;
}
}
for (size_t i = 0; i < table->size; ++i) {
/* autoinvalidate if the entry lifetime is not set to not expire */
if (table->entries[i].lifetime != FIB_LIFETIME_NO_EXPIRE) {
/* check if the lifetime expired */
if (table->entries[i].lifetime < now) {
/* remove this entry if its lifetime expired */
table->entries[i].lifetime = 0;
table->entries[i].global_flags = 0;
table->entries[i].next_hop_flags = 0;
table->entries[i].iface_id = KERNEL_PID_UNDEF;
if (table->entries[i].global != NULL) {
universal_address_rem(table->entries[i].global);
table->entries[i].global = NULL;
}
if (table->entries[i].next_hop != NULL) {
universal_address_rem(table->entries[i].next_hop);
table->entries[i].next_hop = NULL;
}
}
}
if ((prefix_size < (dst_size<<3)) && (table->entries[i].global != NULL)) {
int ret_comp = universal_address_compare(table->entries[i].global, dst, &match_size);
/* If we found an exact match */
if (ret_comp == 0 || (is_all_zeros_addr && match_size == 0)) {
entry_arr[0] = &(table->entries[i]);
*entry_arr_size = 1;
/* we will not find a better one so we return */
return 1;
}
else {
/* we try to find the most fitting prefix */
if (ret_comp == 1) {
entry_arr[0] = &(table->entries[i]);
/* we could find a better one so we move on */
ret = 0;
prefix_size = match_size;
match_size = dst_size<<3;
count = 1;
}
}
}
}
*entry_arr_size = count;
return ret;
}