navigation.c

SOS操作系统用于无线传感器网络节点的源代码

/**
 * Module needs to include <module.h>
 */
#include <module.h>

//#define LED_DEBUG
//#include <led_dbg.h>

#include "navigation.h"
#include "flooding.h"
#include "stdio.h"

//by zhou ya jin
#ifdef SOS_DEBUG_BLINK

#undef DEBUG
#define DEBUG(...) //printf(__VA_ARGS__)

#undef DEBUG_PID
#define DEBUG_PID(...)
static void printMem(char*);
#endif



#define BLINK_TIMER_INTERVAL	MS500
#define BLINK_TID               0
/**
 * Module can define its own state
 */
typedef struct {
  uint8_t pid;
  uint8_t state;
} app_state_t;

/**
 * Blink module
 *
 * @param msg Message being delivered to the module
 * @return int8_t SOS status message
 *
 * Modules implement a module function that acts as a message handler.  The
 * module function is typically implemented as a switch acting on the message
 * type.
 *
 * All modules should included a handler for MSG_INIT to initialize module
 * state, and a handler for MSG_FINAL to release module resources.
 */

static int8_t blink_msg_handler(void *start, Message *e);

/**
 * This is the only global variable one can have.
 */
static mod_header_t SOS_MODULE_HEADER mod_header  = {
	.mod_id         = DFLT_APP_ID0,
	.state_size     = sizeof(app_state_t),
        .num_timers     = 1,
	.num_sub_func   = 0,
	.num_prov_func  = 0,
	.platform_type  = HW_TYPE /* or PLATFORM_ANY */,
	.processor_type = MCU_TYPE,
	.code_id        = ehtons(DFLT_APP_ID0),
	.module_handler = blink_msg_handler,
};







typedef struct noderssi{
  struct noderssi * next;
  uint16_t addr;
  uint8_t RSSI;
} noderssi;
typedef struct {
  noderssi * head;
  noderssi *tail;
  uint8_t node_cnt;
}nq_t;
static int8_t is_in_queue(nq_t *q, uint16_t addr,uint8_t RSSI);

static nq_t node_q;
static int8_t is_in_queue(nq_t *q, uint16_t addr,uint8_t RSSI)
{
  int i;
  uint8_t *dupbuff;
  noderssi *head, *newnode;
  head=q->head;
  for(i=1;i<=(q->node_cnt);i++)
  {
    if(head->addr!=addr)
      head=head->next;
  }
  if(i<=(q->node_cnt))
  {
    head->RSSI=RSSI;
    return 0;
  }
  else
  {
    dupbuff = ker_malloc(sizeof(noderssi), DFLT_APP_ID1);
    newnode=(noderssi*)dupbuff;
    newnode->next=NULL;
    newnode->addr=addr;
    newnode->RSSI=RSSI;
    if(q->head!=NULL)
    {
      q->tail->next=newnode;
      q->tail=newnode;
      q->node_cnt+=1;
      return 1;
    }
    else
    {
      q->head=newnode;
      q->tail=newnode;
      q->node_cnt=1;
      return 1;
    }
  }
}







static int8_t blink_msg_handler(void *state, Message *msg)
{
  /**
   * The module is passed in a void* that contains its state.  For easy
   * reference it is handy to typecast this variable to be of the
   * applications state type.  Note that since we are running as a module,
   * this state is not accessible in the form of a global or static
   * variable.
   */
  uint16_t addr;
  uint8_t RSSI;
  app_state_t *s = (app_state_t*)state;

  Message message;
  Message msg1;
  message.did=DFLT_APP_ID0;
  message.sid=DFLT_APP_ID0;
  message.daddr=0x02;
  message.saddr=0x01;
  message.type=MSG_KER_UNKNOWN;
  message.len=3;
  message.data=NULL;
  message.flag=SOS_MSG_SYSTEM_PRIORITY | SOS_MSG_RADIO_IO| SOS_MSG_RELIABLE;
  message.payload[0]='H';
  message.payload[1]='L';
  message.payload[2]='E';
  uint8_t test=0;


  /**
   * Switch to the correct message handler
   */
  switch (msg->type){
	/**
	 * MSG_INIT is used to initialize module state the first time the
	 * module is used.  Many modules set timers at this point, so that
	 * they will continue to receive periodic (or one shot) timer events.
	 */
  case MSG_INIT:
  {
    DEBUG("BLINK START\n");

    s->pid = msg->did;
	  s->state = 0;
	  /**
	   * The timer API takes the following parameters:
	   * - PID of the module the timer is for
	   * - Timer ID (used to distinguish multiple timers of different
	   *   ..types on the same host)
	   * - Type of timer
	   * - Timer delay in
	   */
	  DEBUG_PID(s->pid, "Blink Start\n");
          KER_timer_init(s->pid, BLINK_TID, TIMER_REPEAT);
	  if(msg->data == NULL) {
                  printf("ker timer start!\n");
		  KER_timer_start(s->pid, BLINK_TID, BLINK_TIMER_INTERVAL);
	  } else {
		  //! get uint16_t from msg->data

		  uint16_t period = *(uint16_t*)(msg->data);
		  KER_timer_start(s->pid, BLINK_TID, period);
	  }


          node_q.head=NULL;
          node_q.tail=NULL;
          node_q.node_cnt=0;
          uint8_t *pktpayload;
          uint8_t payloadlen;
          pktpayload = ker_malloc(sizeof(flood_hdr_t), DFLT_APP_ID1);
          payloadlen=sizeof(flood_hdr_t);

          flood_pkt(DFLT_APP_ID1, MSG_GET_DATA, pktpayload,payloadlen,BCAST_ADDRESS);
          break;


  }

  case MSG_PKT_SENDDONE:
    {
      printf("SEND DONE\n");
      msg1 = *((Message*)msg->data);

      if (test)
      { KER_led(LED_YELLOW_ON);test=0;}
      else
      {
        KER_led(LED_YELLOW_OFF);test=1;
      }
    break;

    }
/**
	 * MSG_FINAL is used to shut modules down.  Modules should release all
	 * resources at this time and take care of any final protocol
	 * shutdown.
	 */
  case MSG_FINAL:
	{
	  /**
	   * Stop the timer
	   */
	  KER_timer_stop(s->pid, BLINK_TID);
	  DEBUG_PID(s->pid, "Blink Stop\n");
	  break;
	}


	/**
	 * All timers addressed to this PID, regardless of the timer ID, are of
	 * type MSG_TIMER_TIMEOUT and handled by this handler.  Timers with
	 * different timer IDs can be further distinguished by testing for the
	 * type, as demonstrated in the relay module.
	 */
  case MSG_TIMER_TIMEOUT:
	{
          //printf("TIMEOUT\n");
	  MsgParam* params = (MsgParam*)(msg->data);
          //KER_post(&message);

	  //			LED_DBG(LED_YELLOW_TOGGLE);
	  if (params->byte == BLINK_TID)
		{

		  if(s->state == 1){
			/**
			 * sys_led is SOS system call for LED.
			 */
			//KER_led(LED_YELLOW_OFF);
                        //KER_led(LED_GREEN_ON);
			
		  }
		  else{
			//KER_led(LED_YELLOW_ON);
                        //KER_led(LED_GREEN_OFF);
			
		  }
		  s->state++;
		  if(s->state > 1) s->state = 0;
		}
	  break;
	}



  case MSG_GET_DATA:
	{
	  /**
	   * Stop the timer
	   */
          addr=*((uint16_t*)((uint8_t *)(msg->data)+msg->len-2));
          RSSI=*(((uint8_t *)(msg->data)+msg->len-3));
          is_in_queue(&node_q,addr,RSSI);


	
	  //DEBUG_PID(s->pid, "Blink Stop\n");
	  break;
	}
	/**
	 * The default handler is used to catch any messages that the module
	 * does no know how to handle.
	 */
  default:
    {
      if ((msg->flag)&SOS_MSG_FROM_NETWORK)
      {
        printf("%s\n",msg->payload);
      }
      if (test==1)
      {
        KER_led(LED_YELLOW_ON);test=0;}
      else
      {
        KER_led(LED_YELLOW_OFF);test=1;
      }

      break;
    }
	//return -EINVAL;
  }

  /**
   * Return SOS_OK for those handlers that have successfully been handled.
   */
  return SOS_OK;
}

#ifndef _MODULE_
mod_header_ptr navi_get_header()
{
  return sos_get_header_address(mod_header);
}
#endif