bedrest_sense.c

MANTIS是由科罗拉多大学开发的传感器网络嵌入式操作系统。 这是mantis的0.9.5版本的源码。

//  This file is part of MANTIS OS, Operating System
//  See http://mantis.cs.colorado.edu/
//
//  Copyright (C) 2003,2004,2005 University of Colorado, Boulder
//
//  This program is free software; you can redistribute it and/or
//  modify it under the terms of the mos license (see file LICENSE)

/**************************************************************************/
/* File:    sense.c  (bionet)                                             */
/* Author     Charles Gruenwald III   :   [e-mail]           */
/*   Date: 03/11/04                                                       */
/*                                                                        */
/* Sensing application for bionet sensor nodes.                           */
/**************************************************************************/

#include "mos.h"
#include "msched.h"
#include "led.h"
#include "avr-adc.h"
#include "adc.h"
#include "dev.h"
#include "com.h"
#include "printf.h"
#include "command_daemon.h"
#include "cc1000.h"
#include "node_net_event.h"
#include "node_id.h"
#include "mutex.h"
#include "queue.h"
#include "net.h"
#include "clock.h"
#include "simple_proto.h"

#ifdef ARCH_AVR
#include "dev.h"
#endif

#define ACCEL_BUF_SIZE 40

static comBuf outpacket;

static uint8_t net_addr;
static boolean verbose;
static uint8_t accelx, accely;

static uint32_t send_sleep;
static uint32_t accel_sample_sleep;
static uint8_t send_power;

static uint16_t accel_cb_x [ACCEL_BUF_SIZE];
static uint16_t accel_cb_y [ACCEL_BUF_SIZE];
static mos_mutex_t accel_mutex;
static uint8_t accel_buf_len;

static   uint16_t accelx_avg;
static   uint16_t accely_avg;
static   uint16_t accelx_std;
static   uint16_t accely_std;

static uint16_t Xa;
static uint16_t Xb;
static uint16_t Ya;
static uint16_t Yb;

net_event_t *event;
bedrest_t *packet;

uint32_t sum = 0;

static uint8_t just_sent;

static float scalex;
static float scaley;
static float offsetx;
static float offsety;

/* functions for controlling verbosity */
//static void set_verbose();
//static void set_quiet();

/* sense and send thread */
void sense_and_send();
uint16_t accel_calc();
void accel_sample();

const char cal_info[] ARCH_PROGMEM = "Calibration Info:\n";
const char accel_info[] ARCH_PROGMEM = "Xa\tXb\tYa\tYb\t\n%d\t%d\t%d\t%d\t\n";
const char scale_info[] ARCH_PROGMEM = "Scale(x): %d, Scale(y): %d\n";
const char offset_info[] ARCH_PROGMEM = "Offset(x): %d, Offset(y): %d\n";

void init()
{
   send_sleep = 2000;
   accel_sample_sleep = 500;
   send_power = 128;

   accel_buf_len = 0;
   mos_mutex_init(&accel_mutex);

   dev_open(DEV_AVR_EEPROM);
   dev_ioctl(DEV_AVR_EEPROM, DEV_SEEK, 14);
   dev_read(DEV_AVR_EEPROM, (uint8_t *)&Xa, sizeof(Xa));
   
   dev_ioctl(DEV_AVR_EEPROM, DEV_SEEK, 16);
   dev_read(DEV_AVR_EEPROM, (uint8_t *)&Xb, sizeof(Xb));

   dev_ioctl(DEV_AVR_EEPROM, DEV_SEEK, 18);
   dev_read(DEV_AVR_EEPROM, (uint8_t *)&Ya, sizeof(Ya));

   dev_ioctl(DEV_AVR_EEPROM, DEV_SEEK, 20);
   dev_read(DEV_AVR_EEPROM, (uint8_t *)&Yb, sizeof(Yb));
   dev_close(DEV_AVR_EEPROM);
   
   scaley = (Ya - Yb)/(float)2;
   offsety = ((float)(Ya + Yb)) / ((float)(Ya - Yb));

   scalex = (Xa - Xb)/(float)2;
   offsetx = ((float)(Xa + Xb)) / ((float)(Xa - Xb));

   printf_P(cal_info);
   printf_P(accel_info, Xa, Xb, Ya, Yb);
   printf_P(scale_info, (uint16_t)scalex, (uint16_t)scaley);
   printf_P(offset_info, (uint16_t)offsetx, (uint16_t)offsety);

   just_sent = false;
}

const char led_info[] ARCH_PROGMEM = "got to led_disp %C\n";

void led_disp(void *p)
{
   printf_P(led_info, *(uint8_t *)p);
   mos_led_display(*(uint8_t *)p);
}

void click(void *p)
{
   uint8_t status = 1;
   dev_open(DEV_MICA2_SOUNDER);
   dev_write(DEV_MICA2_SOUNDER, &status, 1);
   mos_mdelay(80);
   status = 0;
   dev_write(DEV_MICA2_SOUNDER, &status, 1);
   dev_close(DEV_MICA2_SOUNDER);
}

void start(void)
{
   net_addr = mos_node_id_get ();
   net_init();
   simple_proto_init();
   // command daemon, normal user interaction
   //mos_thread_new(mos_command_daemon, MOS_COMMANDER_STACK_SIZE, PRIORITY_NORMAL);
//   mos_register_function("verbose", set_verbose);
//   mos_register_function("quiet", set_quiet);

   // net daemon, handles commands from base station
   mos_net_daemon_init ();
   mos_register_rf_function (LEDS, led_disp);
   mos_register_rf_function (CLICK, click);
   mos_thread_new(mos_net_daemon, 196, PRIORITY_NORMAL);

   init();

   mos_thread_new(sense_and_send, 196, PRIORITY_NORMAL);
   mos_thread_new(accel_sample, 196, PRIORITY_NORMAL);
   
   verbose = TRUE;
}


const char pkt_info[] ARCH_PROGMEM = "%d\t%d\t%d\t%C\t%C\t%d\t%C\t%d\t\n";

void sense_and_send()
{
   com_ioctl_IFACE_RADIO(CC1000_TX_POWER, send_power);
   
   event = (net_event_t *)outpacket.data;
   event->from = net_addr; //from us
   event->to = 0;          //to the base stattion
   event->event = BEDREST_PACKET;

   packet = (bedrest_t *)&(outpacket.data[6]);

   mos_thread_set_suspend_state(SUSPEND_STATE_SLEEP);

   while(1) {
      outpacket.size = 17;

      dev_open(DEV_MICA2_BATTERY);
      dev_open(DEV_MICA2_TEMP);
      dev_mode(DEV_ADC, DEV_MODE_ON);
      dev_mode(DEV_MICA2_BATTERY, DEV_MODE_ON);

      //light value
      dev_read (DEV_MICA2_LIGHT, &(packet->light), 1);
      
      //temp value
      dev_read(DEV_MICA2_TEMP, &(packet->temp), sizeof(packet->temp));
      
      //accelerometer readings
      mos_mutex_lock(&accel_mutex);
      packet->accel_ms = accel_calc();
      mos_mutex_unlock(&accel_mutex);
      
      packet->avg_accelx = accelx_avg;
      packet->avg_accely = accely_avg;

      //tx power
      packet->txpower = cc1000_get_power();

      //battery
      dev_read(DEV_MICA2_BATTERY, &(packet->battery), sizeof(packet->battery));
      
      dev_mode(DEV_ADC, DEV_MODE_OFF);
      dev_mode(DEV_MICA2_BATTERY, DEV_MODE_OFF);
      dev_close(DEV_MICA2_BATTERY);
      dev_close(DEV_MICA2_TEMP);
      
      if(verbose)
      {
	 printf(pkt_info, event->from, event->to, event->event, packet->light,
		packet->temp, packet->battery);
	 //printf("%d\t%d\t%d\t", event->from, event->to, event->event);
	 //printf("%C\t%C\t%d\t%C\t%d\t", packet->light, packet->temp,
	 //packet->accel_ms, packet->txpower,	packet->battery);
	 //printf("\n");
      }

      net_send(&outpacket, SIMPLE_PROTO_ID, 2, true);

      just_sent = true;
      mos_thread_sleep(send_sleep);      
   }
}

void accel_calc_individual(int16_t *buf, int16_t *avg, int16_t *std)
{
   uint8_t i;

   if (accel_buf_len == 0)
      return;

   sum = 0;

   //calculate average
   for(i = 0; i < accel_buf_len; i++)
   {
      sum += (int16_t)buf[i];
   }

   *avg = ((int16_t)sum / accel_buf_len);

   //calculate std-dev
   sum = 0;
   for(i = 0; i < accel_buf_len; i++) {
      sum += ((uint16_t)buf[i] - (uint16_t)*avg) *
	 ((uint16_t)buf[i] - (uint16_t)*avg);
   }

   *std = ((int16_t)sum / accel_buf_len);
}

uint16_t accel_calc()
{

   uint8_t cnt;
   uint16_t rms;

   if(accel_buf_len == 0)
   {
      return accelx_std + accely_std;
   }
   //calculate the rms
   accel_calc_individual(accel_cb_x, &accelx_avg, &accelx_std);
   accel_calc_individual(accel_cb_y, &accely_avg, &accely_std);

   //empty stored points
   cnt = accel_buf_len;
   accel_buf_len = 0;
   

   accelx = (int16_t)accelx_avg;
   accely = (int16_t)accely_avg;
   rms = accelx_std + accely_std;
//   printf("xa %d ya %d xs %d ys %d ms %d\n", accelx_avg, accely_avg, accelx_std, accely_std, rms);
   return rms;
}

/*
  Let A = Accelerometer output with axis oriented to +1 g
  Let B = Accelerometer output with axis oriented to -1 g then: 
  Sensitivity = [A - B]/2
  Offset = - [A+B] / [A - B]
  16.Record the sensitivity for each axis as a linear scale factor.
  17.Create a linear look up table for acceleration
  Acceleration_G = (ADC / Scale_ADC) + Offset_ADC
  Acceleration_G = (ADC / Scale_ADC) - [A+B] / [A-B]
*/

//#define convert_x(x_sample) (x_sample / 1024) - (Xa+Xb)/(Xa-Xb)
//#define convert_y(y_sample) (y_sample / 1024) - (Ya+Yb)/(Ya-Yb)

uint16_t convert_x(uint16_t x_sample)
{
   float temp;
   temp = ((float)x_sample) / ((float)scalex) - offsetx;
   return (uint16_t)(temp * 1000);
}
uint16_t convert_y(uint16_t y_sample)
{
   float temp;
   temp = ((float)y_sample) / ((float)scaley) - offsety;
   return (uint16_t)(temp * 1000);
}

void accel_sample()
{
   uint16_t i, j;

   mos_thread_set_suspend_state(SUSPEND_STATE_SLEEP);
   
   while(1)
   {
      dev_open(DEV_MICA2_ACCEL_X);
      dev_mode(DEV_ADC, DEV_MODE_ON);
      dev_mode(DEV_MICA2_ACCEL_X, DEV_MODE_ON);

      mos_thread_sleep(accel_sample_sleep / 2);
      dev_read (DEV_MICA2_ACCEL_X, &i, 2);
		
      mos_thread_sleep(accel_sample_sleep / 2);
      dev_read(DEV_MICA2_ACCEL_Y, &j, 2);

      dev_mode(DEV_MICA2_ACCEL_X, DEV_MODE_OFF);
      dev_mode(DEV_ADC, DEV_MODE_OFF);
      dev_close(DEV_MICA2_ACCEL_X);
      
      //printf("%d %d ", i, j);
      
      i = convert_x(i);
      j = convert_y(j);
//      if((int16_t)i < 0){
//	 printf("-%d ", -1*i);
//      } else {
//	 printf("%d ", i);
//      }

//      if((int16_t)j < 0){
//	 printf("-%d\n", -1*j);
//      } else {
//	 printf("%d\n", j);
//      }
	 
      mos_mutex_lock(&accel_mutex);

      if(accel_buf_len < ACCEL_BUF_SIZE)
      {
	    accel_cb_x[accel_buf_len] = i;
	    accel_cb_y[accel_buf_len] = j;
	    accel_buf_len++;
      }
      
      mos_mutex_unlock(&accel_mutex);
   }
}


/*
static void set_verbose(){
   verbose=TRUE;
   printf("\nsrc\tdest\ttype\tlite\ttemp\taccel\ttxpw\tbatt\trssi\n");

}
static void set_quiet  (){ verbose=FALSE; }

*/