shoes/chenjunchao/esb_ptx/motion/send_data.c

#include "send_data.h"

void send_package_data(uint16_t package_num, int16_t right_pos_data[3], int16_t left_pos_data[3], int16_t right_att_data[3], int16_t left_att_data[3],
			int16_t s_zupt, int16_t h_zupt,int16_t is_down, int16_t is_jump, int8_t s_rssi, uint8_t time_stamp)
{
	
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(right_pos_data[0]>>8);
	buf[L++] = (uint8_t)(right_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[1]>>8);
	buf[L++] = (uint8_t)(right_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[2]>>8);
	buf[L++] = (uint8_t)(right_pos_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[0]>>8);
	buf[L++] = (uint8_t)(left_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[1]>>8);
	buf[L++] = (uint8_t)(left_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[2]>>8);
	buf[L++] = (uint8_t)(left_pos_data[2]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[0]>>8);
	buf[L++] = (uint8_t)(right_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[1]>>8);
	buf[L++] = (uint8_t)(right_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[2]>>8);
	buf[L++] = (uint8_t)(right_att_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[0]>>8);
	buf[L++] = (uint8_t)(left_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[1]>>8);
	buf[L++] = (uint8_t)(left_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[2]>>8);
	buf[L++] = (uint8_t)(left_att_data[2]>>0);
	
	
	buf[L++] = (uint8_t)(s_zupt * 8 + h_zupt * 4 + is_down * 2 + is_jump * 1);
	
	buf[L++] = (uint8_t)(s_rssi >> 0);
	
	buf[L++] = (uint8_t)(time_stamp);
	
	send_protocol(DEX_NUM,package_num,buf,L);
	

}

void send_data_to_com(int16_t s_pos[3], int16_t h_pos[3], int16_t s_att[3], int16_t h_att[3] , int16_t s_zupt, int16_t h_zupt, int16_t is_down, int16_t is_jump, int8_t s_rssi)
{
	static uint8_t time_stamp = 0;
	
	send_package_data( 0x04, s_pos, h_pos, s_att, h_att ,s_zupt, h_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	time_stamp ++;
}

void send_data_to_host(uint16_t press, int16_t pos_n_offset[3], int16_t att[3], int16_t zupt)
{
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[0]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[0]>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[1]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[1]>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[2]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[2]>>0);
	
	buf[L++] = (uint8_t)(att[0]>>8);
	buf[L++] = (uint8_t)(att[0]>>0);
	
	buf[L++] = (uint8_t)(att[1]>>8);
	buf[L++] = (uint8_t)(att[1]>>0);
	
	buf[L++] = (uint8_t)(att[2]>>8);
	buf[L++] = (uint8_t)(att[2]>>0);
 	
	buf[L++] = (uint8_t)(zupt>>0);
	
	ts++;

	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_acc_mag_to_host(uint16_t press, int16_t accl[3], int16_t magn[3], int16_t zupt)
{
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>0);
	
	buf[L++] = (uint8_t)(accl[0]>>8);
	buf[L++] = (uint8_t)(accl[0]>>0);
	
	buf[L++] = (uint8_t)(accl[1]>>8);
	buf[L++] = (uint8_t)(accl[1]>>0);
	
	buf[L++] = (uint8_t)(accl[2]>>8);
	buf[L++] = (uint8_t)(accl[2]>>0);
	
	buf[L++] = (uint8_t)(magn[0]>>8);
	buf[L++] = (uint8_t)(magn[0]>>0);
	
	buf[L++] = (uint8_t)(magn[1]>>8);
	buf[L++] = (uint8_t)(magn[1]>>0);
	
	buf[L++] = (uint8_t)(magn[2]>>8);
	buf[L++] = (uint8_t)(magn[2]>>0);
 	
	buf[L++] = (uint8_t)(zupt>>0);
	
	ts++;

	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}