static void
broadcast_recv1(struct broadcast_conn *c, const rimeaddr_t *from)
{ for(i=0;i<40;i++) s1[i]='\0';
/*printf("broadcast message received from %d.%d\n",
from->u8[0], from->u8[1]);
printf(" message received '%s'\n", (char *)packetbuf_dataptr());*/
voltage=(char *)packetbuf_dataptr();
rssi=-packetbuf_attr(PACKETBUF_ATTR_RSSI);
//printf("rssi=%u\n",rssi);
sensor = sensors_find(ADC_SENSOR);
if(sensor) {
rv = sensor->value(ADC_SENSOR_TYPE_VDD);
if(rv != -1) {
sane = (rv * 3.75)/ 2047;
dec = sane;
frac = sane - dec;
//printf("Supply=%d.%02u V (%d)\n", dec, (unsigned int)(frac*100), rv);
/* Store rv temporarily in dec so we can use it for the battery */
dec1=dec;
dec = rv;
}}
sprintf(s1,"FE%u,%u,%u,%u,%d,%s,%uEE",from->u8[0],from->u8[1],rimeaddr_node_addr.u8[0],rimeaddr_node_addr.u8[1],rssi,voltage,dec1*100+(unsigned int)(frac*100));
printf("%s\n",s1);
packetbuf_copyfrom(s1, sizeof(s1));
multihop_send(&multihop, &to);
//printf("send\n");
}
void
test_sensor_find(char* name, struct sensors_sensor *psensor)
{
TEST_CODE();
psensor = sensors_find(name);
TEST_NEQ(psensor, NULL);
}
void puttemperature()
{
int rv;
struct sensors_sensor * sensor;
float sane = 0;
int dec;
float frac;
sensor = (struct sensors_sensor *) sensors_find(ADC_SENSOR);
if(sensor)
{
rv = sensor->value(ADC_SENSOR_TYPE_TEMP);
if(rv != -1)
{
sane = 11 + ((rv - 1480) / 4.5);
//sane = ((rv-1367.5)/4.5)-4;
dec = (int)sane;
frac = sane - dec;
printf("Temp=%d.%d C (%d)\r\n", dec, (unsigned int)(frac*100), rv);
}
}
else
putstring("get ADC value failed\r\n");
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(batmon_process, ev, data)
{
PROCESS_BEGIN();
PRINTF("BatMon\n", sizeof(r));
s = sensors_find(ADC_SENSOR);
if (!s) {
PRINTF("BatMon: ADC not found\n");
PROCESS_EXIT();
}
n740_analog_deactivate();
m25p16_res();
n740_analog_activate();
/* Find last written location */
if(find_gap() == -1) {
PRINTF("BatMon: Flash storage full\n");
PROCESS_EXIT();
}
etimer_set(&et, BATMON_LOG_PERIOD * CLOCK_SECOND);
while(1) {
PROCESS_YIELD();
if(ev == PROCESS_EVENT_TIMER && etimer_expired(&et)) {
batmon_log(LOG_TRIGGER_PERIODIC);
}
}
PROCESS_END();
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(udp_server_process, ev, data)
{
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
static struct sensors_sensor *b1;
static struct sensors_sensor *b2;
#endif
PROCESS_BEGIN();
putstring("Starting UDP server\n");
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
putstring("Button 1: Print RIME stats\n");
putstring("Button 2: Reboot\n");
#endif
#if SERVER_RPL_ROOT
create_dag();
#endif
server_conn = udp_new(NULL, UIP_HTONS(0), NULL);
udp_bind(server_conn, UIP_HTONS(3000));
PRINTF("Listen port: 3000, TTL=%u\n", server_conn->ttl);
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
b1 = sensors_find(BUTTON_1_SENSOR);
b2 = sensors_find(BUTTON_2_SENSOR);
#endif
while(1) {
PROCESS_YIELD();
if(ev == tcpip_event) {
tcpip_handler();
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
} else if(ev == sensors_event && data != NULL) {
if(data == b1) {
print_stats();
} else if(data == b2) {
watchdog_reboot();
}
#endif /* (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON) */
}
}
PROCESS_END();
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(udp_server_process, ev, data)
{
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
static struct sensors_sensor *b1;
static struct sensors_sensor *b2;
#endif
PROCESS_BEGIN();
putstring("Starting UDP server\n");
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
putstring("Button X: Toggle LED X\n");
#endif
server_conn = udp_new(NULL, UIP_HTONS(0), NULL);
udp_bind(server_conn, UIP_HTONS(SERVER_PORT));
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
b1 = sensors_find(BUTTON_1_SENSOR);
b2 = sensors_find(BUTTON_2_SENSOR);
#endif
while(1) {
PROCESS_YIELD();
if(ev == tcpip_event) {
tcpip_handler();
#if (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON)
} else if(ev == sensors_event && data != NULL) {
if(data == b1) {
leds_toggle(LEDS_GREEN);
} else if(data == b2) {
leds_toggle(LEDS_RED);
}
#endif /* (CONTIKI_TARGET_SENSINODE && BUTTON_SENSOR_ON) */
}
}
PROCESS_END();
}
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(sensors_test_process, ev, data)
{
static struct etimer et;
/* Sensor Values */
static int rv;
static struct sensors_sensor * sensor;
static float sane = 0;
static int dec;
static float frac;
PROCESS_BEGIN();
PRINTF("========================\n");
PRINTF("Starting Sensor Example.\n");
PRINTF("========================\n");
/* Set an etimer. We take sensor readings when it expires and reset it. */
etimer_set(&et, CLOCK_SECOND * 2);
while (1) {
PROCESS_WAIT_EVENT_UNTIL(etimer_expired(&et));
/*
* Request some ADC conversions
* Return value -1 means sensor not available or turned off in conf
*/
sensor = sensors_find(ADC_SENSOR);
if (sensor) {
PRINTF("------------------\n");
leds_on(LEDS_RED);
/*
* Temperature:
* Using 1.25V ref. voltage (1250mV).
* Typical AD Output at 25°C: 1480
* Typical Co-efficient : 4.5 mV/°C
*
* Thus, at 12bit decimation (and ignoring the VDD co-efficient as well
* as offsets due to lack of calibration):
*
* AD - 1480
* T = 25 + ---------
* 4.5
*/
rv = sensor->value(ADC_SENSOR_TYPE_TEMP);
if(rv != -1) {
sane = 25 + ((rv - 1480) / 4.5);
dec = sane;
frac = sane - dec;
PRINTF(" Temp=%d.%02u C (%d)\n", dec, (unsigned int)(frac*100), rv);
}
/*
* Power Supply Voltage.
* Using 1.25V ref. voltage.
* AD Conversion on VDD/3
*
* Thus, at 12bit resolution:
*
* ADC x 1.25 x 3
* Supply = -------------- V
* 2047
*/
rv = sensor->value(ADC_SENSOR_TYPE_VDD);
if(rv != -1) {
sane = rv * 3.75 / 2047;
dec = sane;
frac = sane - dec;
PRINTF("Supply=%d.%02u V (%d)\n", dec, (unsigned int)(frac*100), rv);
/* Store rv temporarily in dec so we can use it for the battery */
dec = rv;
}
/*
* Battery Voltage - ToDo
* rv = sensor->value(ADC_SENSOR_TYPE_BATTERY);
*/
leds_off(LEDS_RED);
}
etimer_reset(&et);
}
PROCESS_END();
}
PROCESS_THREAD(ahrs_process,ev,data)
{
static hrclock_t timestamp;
static float gx,gy,gz,ax,ay,az,mx,my,mz,td;
static struct sensors_sensor *gyr, *acc, *mag;
if(ev == PROCESS_EVENT_EXIT) { /* switch off sampling */
if (gyr!=NULL) gyr->configure(SENSORS_ACTIVE,0);
if (acc!=NULL) acc->configure(SENSORS_ACTIVE,0);
if (mag!=NULL) mag->configure(SENSORS_ACTIVE,0);
} else if (ev == sensors_event) {
if (data==acc) {
ax = acc->value(ACC_VALUE_X);
ay = acc->value(ACC_VALUE_Y);
az = acc->value(ACC_VALUE_Z);
} else if (data==mag) {
mx = magcal(mag,MAG_VALUE_X);
my = magcal(mag,MAG_VALUE_Y);
mz = magcal(mag,MAG_VALUE_Z);
} else if (data==gyr) {
gx = gyro2rad(gyr->value(GYRO_VALUE_X));
gy = gyro2rad(gyr->value(GYRO_VALUE_Y));
gz = gyro2rad(gyr->value(GYRO_VALUE_Z));
}
}
PROCESS_BEGIN();
/* initialize sensors */
gyr = sensors_find(GYRO_SENSOR);
acc = sensors_find(ACC_SENSOR);
mag = sensors_find(MAG_SENSOR);
if (gyr==NULL) {
strncpy(msg, "gyro not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else if (acc==NULL) {
strncpy(msg, "accelerometer not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else if (mag==NULL) {
strncpy(msg, "compass not found\n",sizeof(msg));
process_exit(&ahrs_process);
} else {
gyr->configure(SENSORS_ACTIVE,1);
acc->configure(SENSORS_ACTIVE,1);
mag->configure(SENSORS_ACTIVE,1);
}
/* configure sensors, gyro to 2000dps@200Hz with bandpass at 15-70Hz, bandpass
* for zero-rate removal! acc to 100Hz and 1Hz high-pass */
gyr->configure(GYRO_L3G_RANGE,GYRO_L3GVAL_2000DPS);
gyr->configure(GYRO_L3G_DRATE,GYRO_L3GVAL_200_70HZ);
gyr->configure(GYRO_L3G_HIGHPASS,GYRO_L3GVAL_HP0); // 15Hz@200Hz with ref
gyr->configure(GYRO_L3G_FILTER,GYRO_L3GVAL_BOTH);
acc->configure(ACC_LSM303_RANGE,ACC_LSM303VAL_2G);
acc->configure(ACC_LSM303_DRATE,ACC_LSM303VAL_100HZ);
acc->configure(ACC_LSM303_HIGHPASS,ACC_LSM303VAL_HP1);
acc->configure(ACC_LSM303_FILTER,ACC_LSM303VAL_BOTH);
/* XXX: ignore first measurements */
while (gx==0 || mx==0 || ax==0)
PROCESS_YIELD_UNTIL(ev==sensors_event);
/* reset quaternion and compass calibration */
q0=1; q1=q2=q3=exInt=eyInt=ezInt=0;
magcal(NULL,0);
while (1) { /* estimate continously and write into msg */
timestamp = clock_hrtime();
PROCESS_YIELD_UNTIL(ev==sensors_event);
/* calculate sampling time */
td = (clock_hrtime()-timestamp) / 1.e6;
/* update estimate */
AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,td);
//IMUupdate(gx,gy,gz,ax,ay,az,td);
/* rebuild msg string */
{
char *s = msg;
s += ftoa(td,s);
*s++ = ' ';
s += ftoa(q0,s);
*s++ = ' ';
s += ftoa(q1,s);
*s++ = ' ';
s += ftoa(q2,s);
*s++ = ' ';
s += ftoa(q3,s);
*s++ = ' ';
s += ftoa(exInt,s);
*s++ = ' ';
s += ftoa(eyInt,s);
*s++ = ' ';
s += ftoa(ezInt,s);
*s++ = '\n';
*s = '\0';
}
}
PROCESS_END();
}
PROCESS_THREAD(acc_process, ev, data)
{
PROCESS_BEGIN();
// just wait shortly to be sure sensor is available
etimer_set(&timer, CLOCK_SECOND * 0.05);
PROCESS_YIELD();
// get pointer to sensor
static const struct sensors_sensor *acc_sensor;
acc_sensor = sensors_find("Acc");
{
// activate and check status
uint8_t status = SENSORS_ACTIVATE(*acc_sensor);
if (status == 0) {
printf("Error: Failed to init accelerometer, aborting...\n");
PROCESS_EXIT();
}
// configure
acc_sensor->configure(ACC_CONF_SENSITIVITY, ACC_2G);
acc_sensor->configure(ACC_CONF_DATA_RATE, ACC_100HZ);
}
static const struct sensors_sensor *gyro_sensor;
gyro_sensor = sensors_find("Gyro");
{
// get pointer to sensor
// activate and check status
uint8_t status = SENSORS_ACTIVATE(*gyro_sensor);
if (status == 0) {
printf("Error: Failed to init gyroscope, aborting...\n");
PROCESS_EXIT();
}
// configure
gyro_sensor->configure(GYRO_CONF_SENSITIVITY, GYRO_250DPS);
gyro_sensor->configure(GYRO_CONF_DATA_RATE, GYRO_100HZ);
}
SENSORS_ACTIVATE(button_sensor);
etimer_set(&timer, CLOCK_SECOND * 0.01);
static int on;
static short accData[3], gyroData[3];
static double pitch = 0, roll = 0;
while (1) {
if(ev == sensors_event && data == &button_sensor) {
on = !on;
} else if(ev == PROCESS_EVENT_TIMER) {
accData[0] = acc_sensor->value(ACC_X);
accData[1] = acc_sensor->value(ACC_Y);
accData[2] = acc_sensor->value(ACC_Z);
gyroData[0] = gyro_sensor->value(GYRO_X);
gyroData[1] = gyro_sensor->value(GYRO_Y);
gyroData[2] = gyro_sensor->value(GYRO_Z);
ComplementaryFilter(accData, gyroData, &pitch, &roll);
// printf("%d %d\n", (int) (pitch), (int) (accData[1]));
// read and output values
// printf("%d %d %d %d %d %d %d\n",
// acc_sensor->value(ACC_X),
// acc_sensor->value(ACC_Y),
// acc_sensor->value(ACC_Z),
// gyro_sensor->value(GYRO_X),
// gyro_sensor->value(GYRO_Y),
// gyro_sensor->value(GYRO_Z),
// on);
etimer_reset(&timer);
}
PROCESS_YIELD();
// PROCESS_PAUSE();
}
// deactivate (never reached here)
SENSORS_DEACTIVATE(*acc_sensor);
SENSORS_DEACTIVATE(*gyro_sensor);
SENSORS_DEACTIVATE(button_sensor);
PROCESS_END();
}
