[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 make_packet(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
|
||||
int receive_position(void) ;
|
||||
#endif // ENABLE_RECEIVE_POSITION
|
||||
|
@ -458,97 +460,36 @@ void send_request()
|
|||
void make_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
|
||||
{
|
||||
printf("Preparing calibration request packet...\n") ;
|
||||
|
||||
offset = 0 ;
|
||||
packet_size =
|
||||
sizeof(uint8_t) * 2 + sizeof(owl_timestamp) + sizeof(float) * 3 +
|
||||
sizeof(uint16_t) * 2 ;
|
||||
add_padding() ;
|
||||
packet = malloc(packet_size) ;
|
||||
|
||||
// Packet type:
|
||||
memset(&packet[offset++], OWL_REQUEST_CALIBRATION, 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) ;
|
||||
// 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) ;
|
||||
offset = initialise_common_fields(OWL_REQUEST_CALIBRATION) ;
|
||||
offset += initialise_calibration_fields(offset) ;
|
||||
}
|
||||
|
||||
else // Standard packet
|
||||
{
|
||||
printf("Preparing request packet...\n") ;
|
||||
|
||||
offset = 0 ;
|
||||
packet_size =
|
||||
sizeof(uint8_t) + sizeof(owl_timestamp) + sizeof(uint16_t) * 2 ;
|
||||
add_padding() ;
|
||||
packet = malloc(packet_size) ;
|
||||
|
||||
// Packet type:
|
||||
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) ;
|
||||
offset = initialise_common_fields(OWL_REQUEST_NORMAL) ;
|
||||
}
|
||||
|
||||
// Initialize padding bytes with 0xFF:
|
||||
while (offset < packet_size)
|
||||
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)
|
||||
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