Show mote_packet.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.
*
*/
/*
* use a query device to simulate a motor driver, which accept and interprets
* commands to the motor
*/
#include "motors/motor_nims.h"
#include "mote_nims.h"
#define MSG_SIZE 36
#define SUC_RET_VAL "0\n"
typedef enum mote_command {
NONE,
MOTE_SEND,
MOTE_RECV
} mote_command_t;
static int calculateCRC(char * packet, int count);
void mote_usage(query_context_t *q, buf_t *buf)
{
bufprintf(buf, "command to send/recv packets\n");
}
void mote_recv_process(mote_nims_state_t *mote)
{
pd_context_t * pd= mote->pd_ref;
// buf_t *bin = qdev_curr_bin(context);
elog( LOG_NOTICE, "mote_recv_process called\n");
fprintf( stderr, "Before bufprint: mote_recv: L67\n" );
fflush( stderr );
// bufprintf(print, "Reach (%.2f, %.2f)\n", mcs->pos.cx, mcs->pos.cy );
// bufprintf(print, "recv pcaktes\n");
// qdev_reply( context, QUERY_DONE);
pd_receive( pd, "recv\n", sizeof("recv\n"));
// buf_free( print );
}
/*
int mote_process(query_context_t *q, char *command, size_t size,
buf_t *print, buf_t *bin)
*/
int mote_process(pd_context_t *pd, const void *packet_command,
int packetlen, int loop_needed)
{
int i;
int tx = -1;
int ty = -1;
int tz = -1;
char * command = (char *) packet_command;
size_t size = packetlen;
parser_state_t p_state = {
input: command,
input_len: size
};
/*
float cx, cy;
motor_cmd_return_state_t ret;
motor_global_info_t *m;
int move_flag = 1;
*/
char packet[36];
mote_nims_state_t *nims_mote;
short int crc;
mote_command_t command_type = NONE;
elog( LOG_NOTICE, "mote_process called, '%s'\n", command);
nims_mote = (mote_nims_state_t *) pd_data( pd );
//initialize them to negative value, suppose
//valid target coordinated are non-negative.
while (misc_parse_next_kvp(&p_state) >= 0) {
/* check for the key 'move' */
if ((strcmp("send", p_state.key) == 0) || (strcmp("recv", p_state.key) == 0)) {
if (strcmp("send", p_state.key) == 0)
{
command_type = MOTE_SEND;
fprintf( stderr, "SENDING\n");
}
else if (strcmp("recv", p_state.key) == 0)
{
command_type = MOTE_RECV;
fprintf( stderr, "RECVING\n");
}
}
if ( strcmp("a1", p_state.key) == 0) {
/* get the x coordinate from the value */
if (p_state.value) {
tx= (int) (atoi(p_state.value));
}
}
if ( strcmp("a2", p_state.key) == 0) {
/* get the y coordinate from the value */
if (p_state.value) {
ty= (int) (atoi(p_state.value));
}
}
if ( strcmp("a3", p_state.key) == 0) {
/* get the y coordinate from the value */
if (p_state.value) {
tz= (int) (atoi(p_state.value));
}
}
}
// mcs = (motor_nims_state_t *) qdev_data(q);
if ( (command_type == MOTE_SEND) && (tx>=0) && (ty>=0) && (tz>=0))
{
memset( packet, 0, 36 );
packet[0] = 0xff; //address : first part
packet[1] = 0xff; // second part
packet[2] = 16; //type : has to be <= 127
packet[3] = 99; //group
packet[4] = 4; //length
packet[5] = (char) tx;
packet[6] = (char) ty;
packet[7] = (char) tz & 0xff;
packet[8] = (char) (tz >> 8) & 0xff;
crc = (short)calculateCRC(packet, MSG_SIZE - 2);
packet[MSG_SIZE - 2] = (char)(crc & 0xff); //34
packet[MSG_SIZE - 1] = (char)((crc >> 8) & 0xff); //35
write( nims_mote->serial_fd, packet, 36);
// fprintf(stderr, "after packets sent\n" );
// bufprintf(print, "0\n" );
// return QUERY_DONE;
pd_receive( pd, SUC_RET_VAL, sizeof(SUC_RET_VAL));
}
else if (command_type == MOTE_RECV)
{
for (i=1; i<10; i++)
{
fprintf( stderr, "call mote_recv_process once\n" );
mote_recv_process(nims_mote);
}
}
return 0;
// return QUERY_NO_REPLY;
// return QUERY_DONE;
}
static
int calculateCRC(char * packet, int count)
{
int crc = 0;
int index = 0;
int i;
while (count > 0) {
crc = crc ^ (int) packet[index] << 8;
index++;
i = 8;
do {
if ((crc & 0x8000) == 0x8000)
crc = crc << 1 ^ 0x1021;
else
crc = crc << 1;
} while(--i != 0);
count --;
}
return crc;
}
See more files for this project here