[Client] Refactor make_packet()
This commit is contained in:
parent
775687a1dc
commit
412b77d37f
|
@ -55,6 +55,8 @@ void create_socket(void) ;
|
||||||
void send_request(void) ;
|
void send_request(void) ;
|
||||||
void make_packet(void) ;
|
void make_packet(void) ;
|
||||||
void add_padding(void) ;
|
void add_padding(void) ;
|
||||||
|
uint_fast16_t initialise_common_fields(uint_fast8_t packet_type) ;
|
||||||
|
uint_fast16_t initialise_calibration_fields(uint_fast16_t offset) ;
|
||||||
#ifdef ENABLE_RECEIVE_POSITION
|
#ifdef ENABLE_RECEIVE_POSITION
|
||||||
int receive_position(void) ;
|
int receive_position(void) ;
|
||||||
#endif // ENABLE_RECEIVE_POSITION
|
#endif // ENABLE_RECEIVE_POSITION
|
||||||
|
@ -458,97 +460,36 @@ void send_request()
|
||||||
void make_packet()
|
void make_packet()
|
||||||
{
|
{
|
||||||
uint_fast16_t offset ; // Index used to create the packet
|
uint_fast16_t offset ; // Index used to create the packet
|
||||||
owl_timestamp request_time ;
|
|
||||||
char request_time_str[OWL_TIMESTAMP_STRLEN] ;
|
|
||||||
uint16_t npkt ;
|
|
||||||
|
|
||||||
// Get the current time and copy it as a string before to switch it to
|
|
||||||
// network endianess:
|
|
||||||
owl_timestamp_now(&request_time) ;
|
|
||||||
owl_timestamp_to_string(&request_time, request_time_str) ;
|
|
||||||
owl_hton_timestamp(&request_time) ;
|
|
||||||
|
|
||||||
if (is_calibration_request) // Calibration packet
|
if (is_calibration_request) // Calibration packet
|
||||||
{
|
{
|
||||||
printf("Preparing calibration request packet...\n") ;
|
printf("Preparing calibration request packet...\n") ;
|
||||||
|
|
||||||
offset = 0 ;
|
|
||||||
packet_size =
|
packet_size =
|
||||||
sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 +
|
sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 +
|
||||||
sizeof(uint16_t) * 2 ;
|
sizeof(uint16_t) * 2 ;
|
||||||
add_padding() ;
|
add_padding() ;
|
||||||
packet = malloc(packet_size) ;
|
packet = malloc(packet_size) ;
|
||||||
|
|
||||||
// Packet type:
|
offset = initialise_common_fields(OWL_REQUEST_CALIBRATION) ;
|
||||||
memset(&packet[offset++], OWL_REQUEST_CALIBRATION, 1) ;
|
offset += initialise_calibration_fields(offset) ;
|
||||||
// Number of the current packet (1 for the first):
|
|
||||||
npkt = htons(1u) ;
|
|
||||||
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
|
||||||
offset += sizeof(uint16_t) ;
|
|
||||||
// Number of packets:
|
|
||||||
npkt = htons(options.nb_pkt) ;
|
|
||||||
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
|
||||||
offset += sizeof(uint16_t) ;
|
|
||||||
// Request time:
|
|
||||||
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
|
||||||
offset += sizeof(request_time) ;
|
|
||||||
// Direction:
|
|
||||||
packet[offset++] = options.direction ;
|
|
||||||
#ifdef DEBUG
|
|
||||||
printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
|
|
||||||
packet[offset - 1], options.x, options.y, options.z) ;
|
|
||||||
#endif // DEBUG
|
|
||||||
// Convert the coordinates to the network endianess:
|
|
||||||
options.x = owl_htonf(options.x) ;
|
|
||||||
options.y = owl_htonf(options.y) ;
|
|
||||||
options.z = owl_htonf(options.z) ;
|
|
||||||
// Copy the coordinates to the packet:
|
|
||||||
memcpy(&packet[offset], &options.x, sizeof(float)) ;
|
|
||||||
offset += sizeof(float) ;
|
|
||||||
memcpy(&packet[offset], &options.y, sizeof(float)) ;
|
|
||||||
offset += sizeof(float) ;
|
|
||||||
memcpy(&packet[offset], &options.z, sizeof(float)) ;
|
|
||||||
offset += sizeof(float) ;
|
|
||||||
// Convert the coordinates back to the host endianess (mandatory
|
|
||||||
// in flood mode):
|
|
||||||
options.x = owl_ntohf(options.x) ;
|
|
||||||
options.y = owl_ntohf(options.y) ;
|
|
||||||
options.z = owl_ntohf(options.z) ;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
else // Standard packet
|
else // Standard packet
|
||||||
{
|
{
|
||||||
printf("Preparing request packet...\n") ;
|
printf("Preparing request packet...\n") ;
|
||||||
|
|
||||||
offset = 0 ;
|
|
||||||
packet_size =
|
packet_size =
|
||||||
sizeof(uint8_t) + sizeof(owl_timestamp) + sizeof(uint16_t) * 2 ;
|
sizeof(uint8_t) + sizeof(owl_timestamp) + sizeof(uint16_t) * 2 ;
|
||||||
add_padding() ;
|
add_padding() ;
|
||||||
packet = malloc(packet_size) ;
|
packet = malloc(packet_size) ;
|
||||||
|
|
||||||
// Packet type:
|
offset = initialise_common_fields(OWL_REQUEST_NORMAL) ;
|
||||||
memset(&packet[offset++], OWL_REQUEST_NORMAL, 1) ;
|
|
||||||
// Number of the current packet (1 for the first):
|
|
||||||
npkt = htons(1u) ;
|
|
||||||
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
|
||||||
offset += sizeof(uint16_t) ;
|
|
||||||
// Number of packets:
|
|
||||||
npkt = htons(options.nb_pkt) ;
|
|
||||||
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
|
||||||
offset += sizeof(uint16_t) ;
|
|
||||||
// Request time:
|
|
||||||
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
|
||||||
offset += sizeof(request_time) ;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Initialize padding bytes with 0xFF:
|
// Initialize padding bytes with 0xFF:
|
||||||
while (offset < packet_size)
|
while (offset < packet_size)
|
||||||
packet[offset++] = 0xFF ;
|
packet[offset++] = 0xFF ;
|
||||||
|
|
||||||
printf("Packet timestamp: %s\n"
|
|
||||||
"Packet size: %"PRIuFAST16"\n",
|
|
||||||
request_time_str,
|
|
||||||
packet_size) ;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -560,6 +501,81 @@ void add_padding()
|
||||||
{
|
{
|
||||||
if (options.pkt_size > packet_size)
|
if (options.pkt_size > packet_size)
|
||||||
packet_size = options.pkt_size ;
|
packet_size = options.pkt_size ;
|
||||||
|
|
||||||
|
printf("Packet size: %"PRIuFAST16"\n", packet_size) ;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Initialises the fields of a normal positioning request.
|
||||||
|
*/
|
||||||
|
uint_fast16_t initialise_common_fields(uint_fast8_t packet_type)
|
||||||
|
{
|
||||||
|
uint_fast16_t offset = 0 ;
|
||||||
|
uint16_t npkt ;
|
||||||
|
owl_timestamp request_time ;
|
||||||
|
char request_time_str[OWL_TIMESTAMP_STRLEN] ;
|
||||||
|
|
||||||
|
// Get the current time and copy it as a string before to switch it to
|
||||||
|
// network endianess:
|
||||||
|
owl_timestamp_now(&request_time) ;
|
||||||
|
owl_timestamp_to_string(&request_time, request_time_str) ;
|
||||||
|
owl_hton_timestamp(&request_time) ;
|
||||||
|
|
||||||
|
// Packet type:
|
||||||
|
memset(&packet[offset++], packet_type, 1) ;
|
||||||
|
|
||||||
|
// Number of the current packet (1 for the first):
|
||||||
|
npkt = htons(1u) ;
|
||||||
|
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
||||||
|
offset += sizeof(uint16_t) ;
|
||||||
|
|
||||||
|
// Number of packets:
|
||||||
|
npkt = htons(options.nb_pkt) ;
|
||||||
|
memcpy(&packet[offset], &npkt, sizeof(uint16_t)) ;
|
||||||
|
offset += sizeof(uint16_t) ;
|
||||||
|
|
||||||
|
// Request time:
|
||||||
|
memcpy(&packet[offset], &request_time, sizeof(request_time)) ;
|
||||||
|
offset += sizeof(request_time) ;
|
||||||
|
|
||||||
|
printf("Packet timestamp: %s\n", request_time_str) ;
|
||||||
|
|
||||||
|
return offset ;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Initialises the calibration data fields.
|
||||||
|
*/
|
||||||
|
uint_fast16_t initialise_calibration_fields(uint_fast16_t offset)
|
||||||
|
{
|
||||||
|
float x, y, z ;
|
||||||
|
|
||||||
|
// Direction:
|
||||||
|
packet[offset++] = options.direction ;
|
||||||
|
|
||||||
|
#ifdef DEBUG
|
||||||
|
printf("Direction = %d, X = %f, Y = %f, Z = %f\n",
|
||||||
|
packet[offset - 1], options.x, options.y, options.z) ;
|
||||||
|
#endif // DEBUG
|
||||||
|
|
||||||
|
// Convert the coordinates to the network endianess:
|
||||||
|
x = owl_htonf(options.x) ;
|
||||||
|
y = owl_htonf(options.y) ;
|
||||||
|
z = owl_htonf(options.z) ;
|
||||||
|
|
||||||
|
// Copy the coordinates to the packet:
|
||||||
|
memcpy(&packet[offset], &x, sizeof(float)) ;
|
||||||
|
offset += sizeof(float) ;
|
||||||
|
memcpy(&packet[offset], &y, sizeof(float)) ;
|
||||||
|
offset += sizeof(float) ;
|
||||||
|
memcpy(&packet[offset], &z, sizeof(float)) ;
|
||||||
|
offset += sizeof(float) ;
|
||||||
|
|
||||||
|
return offset ;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue