Show SwMagSensor.c syntax highlighted
/*
*
* Copyright (c) 2003 The Regents of the University of California. All
* rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* - Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* - Neither the name of the University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
/*
* Copyright (c) 2001 The Regents of the University of California.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* 3. All advertising materials mentioning features or use of this
* software must display the following acknowledgement:
* This product includes software developed by Networked &
* Embedded Systems Lab at UCLA
* 4. Neither the name of the University nor that of the Laboratory
* may be used to endorse or promote products derived from this
* software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS''
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS
* OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
* USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
* OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* Author: Simon Han (simonhan@ee.ucla.edu)
*/
#include "sensorware.h"
typedef struct{
int samplePeriod;
int bufSize;
int wakeupPeriod;
int threshold;
} SwMagSensor;
typedef struct sensor_value_struct{
int fd;
// current sensor reading
volatile int x;
volatile int y;
volatile int z;
// previous sensor reading
int p_x;
int p_y;
int p_z;
// baseline sensor reading
int b_x;
int b_y;
int b_z;
// error range
int e_x;
int e_y;
int e_z;
} SwMagSensorState;
void *swMagSensorThread(void *arg);
int swMagSensorPs(Tcl_Interp *interp, int argc, char *argv[]);
int swMagSensorLs(Tcl_Interp *interp, int argc, char *argv[]);
int swMagSensorInterest(Tcl_Interp *interp, int argc, char *argv[],
void **dataPtr);
int swMagSensorActivate(SwInterest *interest);
int swMagSensorDeactivate(SwInterest *interest);
int swMagSensorDispose(void *param);
// register magnetic sensor device
static SwDeviceOp op = {
interest: swMagSensorInterest,
ps: swMagSensorPs,
ls: swMagSensorLs,
activate: swMagSensorActivate,
deactivate: swMagSensorDeactivate,
dispose: swMagSensorDispose,
thread: swMagSensorThread,
};
static swDiffQ dq;
void setline(int fd, int flags, int speed);
int output_sensor(int fd, char *buf, int size);
int read_data_s(int fd, char *buf, int size, int cnt);
int read_data_b(int fd, SwMagSensorState *val);
static SwMagSensorState state;
//static pthread_mutex_t mutex;
//static SwMagSensor sensorList[MAGSENSOR_MAX_SENSOR];
//static int sensorCnt;
void *swMagSensorRead(void *arg){
while(1){
output_sensor(state.fd, "*00P ", 4);
usleep(20000);
read_data_b(state.fd, &state);
}
}
int swMagSensorInit(){
pthread_t thread_id;
char buf[255];
int fd;
int i;
int x, y, z;
int xmax = INT_MIN, ymax = INT_MIN, zmax = INT_MIN,
xmin = INT_MAX, ymin = INT_MAX, zmin = INT_MAX;
SwMagSensorState sensor_value;
#ifdef NOSENSOR
state.fd = -1;
// we fake the sensor here for debugging
state.x = 0;
state.y = 1;
state.z = 2;
//sensorCnt = 0;
swDeviceRegister("magsensor", &op);
return 0;
#endif
if((fd = open("/dev/tts/0", O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0){
perror("unable to open serial device");
state.fd = -1;
return -ENXIO;
}
DEBUG("Serial Port Open for sensor detection!\n");
setline(fd, CS8, B9600);
// device specific initialization
// here, we also try to identify the 'true' baseline
do{
do{
output_sensor(fd, "*00WE ", 5);
//write_enable(fd);
if(read_data_s(fd, buf, 8, 100000) == -1){
DEBUG("no sensor presents\n");
close(fd);
state.fd = -1;
return -ENXIO;
}
}while(strncmp(buf, "OK", 2));
output_sensor(fd, "*00D ", 4);
read_data_s(fd, buf, 8, 0);
}while(strncmp(buf, "OK", 2));
read_data_s(fd, buf, 8, 0);
do{
do{
output_sensor(fd, "*00WE ", 5);
//write_enable(fd);
read_data_s(fd, buf, 8, 0);
}while(strncmp(buf, "OK", 2));
output_sensor(fd, "*00B ", 4);
//output_c(fd, 'B');
read_data_s(fd, buf, 8, 0);
}while(strncmp(buf, "BINARY O", 8));
read_data_s(fd, buf, 2, 0);
while( read_data_s(fd, buf, 1, 1) == 1);
DEBUG("computing base line and error\n");
sensor_value.e_x = 10;
sensor_value.e_y = 10;
sensor_value.e_z = 10;
while(sensor_value.e_x > 5 || sensor_value.e_x < 0 ||
sensor_value.e_y > 5 || sensor_value.e_y < 0 ||
sensor_value.e_z > 5 || sensor_value.e_z < 0){
int wrong_base = 0;
DEBUG("computing...\n");
// time_t t;
// read baseline (x, y, z)
sensor_value.x = 0;
sensor_value.y = 0;
sensor_value.z = 0;
sensor_value.p_x = 0;
sensor_value.p_y = 0;
sensor_value.p_z = 0;
while((sensor_value.x == 0) &&
(sensor_value.y == 0) &&
(sensor_value.z == 0)){
output_sensor(fd, "*00P ", 4);
usleep(20000);
read_data_b(fd, &sensor_value);
}
sensor_value.b_x = sensor_value.x;
sensor_value.b_y = sensor_value.y;
sensor_value.b_z = sensor_value.z;
for(i = 0; i < 100; i++){
output_sensor(fd, "*00P ", 4);
usleep(20000);
read_data_b(fd, &sensor_value);
//printf("reading x = %d, y = %d, z = %d\n", sensor_value.x,
// sensor_value.y, sensor_value.z);
if(abs(sensor_value.b_x - sensor_value.x) > 2 ||
abs(sensor_value.b_y - sensor_value.y) > 2 ||
abs(sensor_value.b_z - sensor_value.z) > 2){
wrong_base = 1;
break;
}
}
if(wrong_base == 1){
//printf("wrong base");
continue;
}
// TODO compute error (x, y, z)
x = 0; y = 0; z = 0;
//printf("before 100 samples... %d\n", time(&t));
for(i = 0; i < 50; i++){
output_sensor(fd, "*00P ", 4);
usleep(20000);
read_data_b(fd, &sensor_value);
x += ((sensor_value.x - sensor_value.b_x) *
(sensor_value.x - sensor_value.b_x));
y += ((sensor_value.y - sensor_value.b_y) *
(sensor_value.y - sensor_value.b_y));
z += ((sensor_value.z - sensor_value.b_z) *
(sensor_value.z - sensor_value.b_z));
if(sensor_value.x > xmax){ xmax = sensor_value.x; }
if(sensor_value.y > ymax){ ymax = sensor_value.y; }
if(sensor_value.z > zmax){ zmax = sensor_value.z; }
if(sensor_value.x < xmin){ xmin = sensor_value.x; }
if(sensor_value.y < ymin){ ymin = sensor_value.y; }
if(sensor_value.z < zmin){ zmin = sensor_value.z; }
// printf("reading x = %d, y = %d, z = %d\n", sensor_value.x,
// sensor_value.y, sensor_value.z);
//printf("reading x = %d, y = %d, z = %d\n", x,
// y, z);
}
//printf("after 100 samples... %d\n", time(&t));
sensor_value.e_x = sqrt(((double)x)/50);
sensor_value.e_y = sqrt(((double)y)/50);
sensor_value.e_z = sqrt(((double)z)/50);
}
if(abs(sensor_value.b_x - xmax) > abs(sensor_value.b_x - xmin)){
sensor_value.e_x = abs(sensor_value.b_x - xmax);
}
else{
sensor_value.e_x = abs(sensor_value.b_x - xmin);
}
if(abs(sensor_value.b_y - ymax) > abs(sensor_value.b_y - ymin)){
sensor_value.e_y = abs(sensor_value.b_y - ymax);
}
else{
sensor_value.e_y = abs(sensor_value.b_y - ymin);
}
if(abs(sensor_value.b_z - zmax) > abs(sensor_value.b_z - zmin)){
sensor_value.e_z = abs(sensor_value.b_z - zmax);
}
else{
sensor_value.e_z = abs(sensor_value.b_z - zmin);
}
DEBUG("\nbase x = %d, y = %d, z = %d\n", sensor_value.b_x,
sensor_value.b_y, sensor_value.b_z);
DEBUG("error x = %d, y = %d, z = %d\n", sensor_value.e_x,
sensor_value.e_y, sensor_value.e_z);
output_sensor(fd, "*00C ", 4);
state = sensor_value;
state.fd = fd;
if(pthread_create(&thread_id, &thread_attr, swMagSensorRead, NULL) != 0){
close(fd);
state.fd = -1;
return -ENOMEM;
}
swDeviceRegister("magsensor", &op);
return 0;
}
// thread responsible for reading sensor data
// based on agent's configuration
void *swMagSensorThread(void *arg){
/*
SwMagSensor *task = &(sensorList[(int)arg]);
SwMessage msg;
SwAddress src;
int hasData = 0;
int msgSize = 0;
int size = strlen(task->name) + 1 + task->bufSize * MAGSENSOR_BUF_UNIT + 1;
int onSec = task->samplePeriod / 1000;
int onUsec = (task->samplePeriod % 1000) * 1000;
int waitSec = (task->wakeupPeriod - task->samplePeriod * task->bufSize) / 1000;
int waitUsec = ((task->wakeupPeriod - task->samplePeriod * task->bufSize)
% 1000) * 1000;
DEBUG("%ld sensor thread start...\n", pthread_self());
src.node = pthread_self();
src.user = -1;
src.family = -1;
src.instance = -1;
swMailConstructMsgHdr(&(msg.hdr), NET_MSG_MORE_DATA, &(task->from), &src,
size);
msg.data = NULL;
DEBUG("onsec = %d, onUsec = %d, waitSec = %d, waitUsec = %d\n",
onSec, onUsec, waitSec, waitUsec);
while(1){
int i = 0;
pthread_testcancel();
if(msg.data == NULL){
while((msg.data = (char *)ckalloc(size)) == NULL){
pthread_testcancel();
}
msgSize = sprintf(msg.data, "%s ", task->name);
hasData = 0;
}
while(i < task->bufSize){
if(task->threshold == 0){
msgSize += sprintf(msg.data + msgSize, "%hd %hd %hd ",
state.x, state.y, state.z);
hasData = 1;
}
else{
int x = state.x;
int y = state.y;
int z = state.z;
if((x > (state.b_x + state.e_x + task->threshold)) ||
(x < (state.b_x - state.e_x - task->threshold)) ||
(y > (state.b_y + state.e_y + task->threshold)) ||
(y < (state.b_y - state.e_y - task->threshold)) ||
(z > (state.b_z + state.e_z + task->threshold)) ||
(z < (state.b_z - state.e_z - task->threshold))){
msgSize += sprintf(msg.data + msgSize, "%hd %hd %hd ",
x, y, z);
hasData = 1;
}
}
if(onSec != 0){ sleep(onSec); }
if(onUsec != 0){ usleep(onUsec); }
i++;
}
if(waitSec != 0){ sleep(waitSec); }
if(waitUsec != 0){ usleep(waitUsec); }
if(hasData){
if(swMailSend(&msg) < 0){
ckfree(msg.data);
}
msg.data = NULL;
}
}
DEBUG("%ld sensor thread finished\n", pthread_self());
*/
char *buf;
swDiffQInit(&dq);
while(1){
SwInterest *interest;
int size;
interest = (SwInterest*)swDiffQWait(&dq);
if(interest != NULL){
size = strlen(interest->namePtr) + 30;
buf = (char *)ckalloc(size);
if(buf != NULL){
sprintf(buf, "%s %hd %hd %hd", interest->namePtr,
state.x, state.y, state.z);
if(swInterestSendMail(interest, buf, size) < 0){
ckfree(buf);
}
}
}
}
pthread_exit(NULL);
}
// event interface
int swMagSensorInterest(Tcl_Interp *interp, int argc, char *argv[],
void **dataPtr){
SwMagSensor *arg;
#ifndef NOSENSOR
if(state.fd == -1){
// sensor is not presented
Tcl_AppendResult(interp, "magsensor: magsensor not available", NULL);
return TCL_ERROR;
}
#endif
arg = (SwMagSensor*)ckalloc(sizeof(SwMagSensor));
if(arg == NULL){
Tcl_AppendResult(interp, SW_ERR_MEM, NULL);
return TCL_ERROR;
}
if(swDeviceParseSensorArg(argc, argv,
&(arg->samplePeriod), &(arg->bufSize),
&(arg->wakeupPeriod), &(arg->threshold)) < 0){
Tcl_AppendResult(interp, "magsensor: invalid parameters", NULL);
return TCL_ERROR;
}
*dataPtr = (void*)arg;
return TCL_OK;
}
// device ps interface
int swMagSensorPs(Tcl_Interp *interp, int argc, char *argv[]){
return TCL_OK;
}
int swMagSensorLs(Tcl_Interp *interp, int argc, char *argv[]){
Tcl_AppendResult(interp, "20\t", NULL);
return TCL_OK;
}
int swMagSensorActivate(SwInterest *interest){
swDiffQNewItem(&dq,
((SwMagSensor*)interest->devParamPtr)->samplePeriod,
(void*)interest);
return 0;
}
int swMagSensorDeactivate(SwInterest *interest){
swDiffQDeleteItem(&dq, (void*)interest);
return 0;
}
// device cancel interface
int swMagSensorDispose(void *param){
ckfree(param);
return 0;
}
/* ***********************************************************************
* sensor device helper functions
* **********************************************************************/
void setline(int fd, int flags, int speed)
{
struct termios t;
tcgetattr(fd, &t);
t.c_cflag = flags | CREAD | HUPCL | CLOCAL;
t.c_iflag = IGNBRK | IGNPAR;
t.c_oflag = 0;
t.c_lflag = 0;
t.c_cc[VMIN ] = 1;
t.c_cc[VTIME] = 0;
cfsetispeed(&t, speed);
cfsetospeed(&t, speed);
tcsetattr(fd, TCSANOW, &t);
}
int output_sensor(int fd, char *buf, int size){
char b[100];
memcpy(b, buf, size);
b[size] = 0x0d;
write(fd, b, size + 1);
return 1;
}
int read_data_s(int fd, char *buf, int size, int cnt)
{
int s = 0;
/*wait for the first character*/
while(1){
//output_c(fd, 'P');
//printf("trying...\n");
if((s = read(fd, buf, size)) > 0){
buf[s]='\0';
//printf("%s\n",buf);
return 1;
}
if(cnt != 0){
cnt--;
if(cnt == 0){
buf[0] = '\0';
return -1;
}
}
}
//printf("%s\n",buf);
return 1;
}
int read_data_b(int fd, SwMagSensorState *val){
char buf[7];
int size = 0;
//gettimeofday(&tv, NULL);
//printf("usec before read = %d\n", tv.tv_usec);
if(((size = read(fd, buf, 7)) == 7) && (buf[6] == 0x0d)){
val->x = (buf[0] << 8) | buf[1];
val->y = (buf[2] << 8) | buf[3];
val->z = (buf[4] << 8) | buf[5];
val->p_x = val->x;
val->p_y = val->y;
val->p_z = val->z;
//DEBUG("x = %d, y = %d, z = %d\n", val->x, val->y, val->z);
}
else{
// interpolation...
val->x = val->p_x;
val->y = val->p_y;
val->z = val->p_z;
//DEBUG("interpolation\n");
}
//gettimeofday(&tv, NULL);
//printf("usec after read = %d\n", tv.tv_usec);
return 0;
}
See more files for this project here