orientus_packets.c
Go to the documentation of this file.
00001 /****************************************************************/
00002 /*                                                              */
00003 /*         Advanced Navigation Packet Protocol Library          */
00004 /*         C Language Dynamic Orientus SDK, Version 1.1         */
00005 /*   Copyright 2013, Xavier Orr, Advanced Navigation Pty Ltd    */
00006 /*                                                              */
00007 /****************************************************************/
00008 /*
00009  * Copyright (C) 2013 Advanced Navigation Pty Ltd
00010  *
00011  * Permission is hereby granted, free of charge, to any person obtaining
00012  * a copy of this software and associated documentation files (the "Software"),
00013  * to deal in the Software without restriction, including without limitation
00014  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00015  * and/or sell copies of the Software, and to permit persons to whom the
00016  * Software is furnished to do so, subject to the following conditions:
00017  *
00018  * The above copyright notice and this permission notice shall be included
00019  * in all copies or substantial portions of the Software.
00020  *
00021  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00022  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00023  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
00024  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00025  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00026  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
00027  * DEALINGS IN THE SOFTWARE.
00028  */
00029  
00030  #include <stdint.h>
00031  #include <string.h>
00032  #include "an_packet_protocol.h"
00033  #include "orientus_packets.h"
00034 
00035 /*
00036  * This file contains functions to decode and encode packets
00037  *
00038  * Decode functions take an an_packet_t and turn it into a type specific
00039  * to that packet so that the fields can be conveniently accessed. Decode
00040  * functions return 0 for success and 1 for failure. Decode functions are
00041  * used when receiving packets.
00042  *
00043  * Example decode
00044  *
00045  * an_packet_t an_packet
00046  * acknowledge_packet_t acknowledge_packet
00047  * ...
00048  * decode_acknowledge_packet(&acknowledge_packet, &an_packet);
00049  * printf("acknowledge id %d with result %d\n", acknowledge_packet.packet_id, acknowledge_packet.acknowledge_result);
00050  *
00051  * Encode functions take a type specific structure and turn it into an
00052  * an_packet_t. Encode functions are used when sending packets. Don't
00053  * forget to free the returned packet with an_packet_free().
00054  *
00055  * Example encode
00056  *
00057  * an_packet_t *an_packet;
00058  * boot_mode_packet_t boot_mode_packet;
00059  * ...
00060  * boot_mode_packet.boot_mode = boot_mode_bootloader;
00061  * an_packet = encode_boot_mode_packet(&boot_mode_packet);
00062  * serial_port_transmit(an_packet_pointer(an_packet), an_packet_size(an_packet));
00063  * an_packet_free(&an_packet);
00064  *
00065  */
00066 
00067 int decode_acknowledge_packet(acknowledge_packet_t *acknowledge_packet, an_packet_t *an_packet)
00068 {
00069         if(an_packet->id == packet_id_acknowledge && an_packet->length == 4)
00070         {
00071                 acknowledge_packet->packet_id = an_packet->data[0];
00072                 memcpy(&acknowledge_packet->packet_crc, &an_packet->data[1], sizeof(uint16_t));
00073                 acknowledge_packet->acknowledge_result = an_packet->data[3];
00074                 return 0;
00075         }
00076         else return 1;
00077 }
00078 
00079 an_packet_t *encode_request_packet(uint8_t requested_packet_id)
00080 {
00081         an_packet_t *an_packet = an_packet_allocate(1, packet_id_request);
00082         an_packet->data[0] = requested_packet_id;
00083         return an_packet;
00084 }
00085 
00086 int decode_boot_mode_packet(boot_mode_packet_t *boot_mode_packet, an_packet_t *an_packet)
00087 {
00088         if(an_packet->id == packet_id_boot_mode && an_packet->length == 1)
00089         {
00090                 boot_mode_packet->boot_mode = an_packet->data[0];
00091                 return 0;
00092         }
00093         else return 1;
00094 }
00095 
00096 an_packet_t *encode_boot_mode_packet(boot_mode_packet_t *boot_mode_packet)
00097 {
00098         an_packet_t *an_packet = an_packet_allocate(1, packet_id_boot_mode);
00099         if(an_packet != NULL)
00100         {
00101                 an_packet->data[0] = boot_mode_packet->boot_mode;
00102         }
00103         return an_packet;
00104 }
00105 
00106 int decode_device_information_packet(device_information_packet_t *device_information_packet, an_packet_t *an_packet)
00107 {
00108         if(an_packet->id == packet_id_device_information && an_packet->length == 24)
00109         {
00110                 memcpy(&device_information_packet->software_version, &an_packet->data[0], sizeof(uint32_t));
00111                 memcpy(&device_information_packet->device_id, &an_packet->data[4], sizeof(uint32_t));
00112                 memcpy(&device_information_packet->hardware_revision, &an_packet->data[8], sizeof(uint32_t));
00113                 memcpy(&device_information_packet->serial_number[0], &an_packet->data[12], 3*sizeof(uint32_t));
00114                 return 0;
00115         }
00116         else return 1;
00117 }
00118 
00119 an_packet_t *encode_restore_factory_settings_packet()
00120 {
00121         uint32_t verification = 0x85429E1C;
00122         an_packet_t *an_packet = an_packet_allocate(4, packet_id_restore_factory_settings);
00123         if(an_packet != NULL)
00124         {
00125                 memcpy(&an_packet->data[0], &verification, sizeof(uint32_t));
00126         }
00127         return an_packet;
00128 }
00129 
00130 an_packet_t *encode_reset_packet()
00131 {
00132         uint32_t verification = 0x21057A7E;
00133         an_packet_t *an_packet = an_packet_allocate(4, packet_id_reset);
00134         if(an_packet != NULL)
00135         {
00136                 memcpy(&an_packet->data[0], &verification, sizeof(uint32_t));
00137         }
00138         return an_packet;
00139 }
00140  
00141 int decode_system_state_packet(system_state_packet_t *system_state_packet, an_packet_t *an_packet)
00142 {
00143         if(an_packet->id == packet_id_system_state && an_packet->length == 100)
00144         {
00145                 memcpy(&system_state_packet->system_status, &an_packet->data[0], sizeof(uint16_t));
00146                 memcpy(&system_state_packet->filter_status, &an_packet->data[2], sizeof(uint16_t));
00147                 memcpy(&system_state_packet->unix_time_seconds, &an_packet->data[4], sizeof(uint32_t));
00148                 memcpy(&system_state_packet->microseconds, &an_packet->data[8], sizeof(uint32_t));
00149                 memcpy(&system_state_packet->orientation[0], &an_packet->data[64], 3 * sizeof(float));
00150                 memcpy(&system_state_packet->angular_velocity[0], &an_packet->data[76], 3 * sizeof(float));
00151                 return 0;
00152         }
00153         else return 1;
00154 }
00155 
00156 int decode_unix_time_packet(unix_time_packet_t *unix_time_packet, an_packet_t *an_packet)
00157 {
00158         if(an_packet->id == packet_id_unix_time && an_packet->length == 8)
00159         {
00160                 memcpy(&unix_time_packet->unix_time_seconds, &an_packet->data[0], sizeof(uint32_t));
00161                 memcpy(&unix_time_packet->microseconds, &an_packet->data[4], sizeof(uint32_t));
00162                 return 0;
00163         }
00164         else return 1;
00165 }
00166 
00167 int decode_status_packet(status_packet_t *status_packet, an_packet_t *an_packet)
00168 {
00169         if(an_packet->id == packet_id_status && an_packet->length == 4)
00170         {
00171                 memcpy(&status_packet->system_status, &an_packet->data[0], sizeof(uint16_t));
00172                 memcpy(&status_packet->filter_status, &an_packet->data[2], sizeof(uint16_t));
00173                 return 0;
00174         }
00175         else return 1;
00176 }
00177 
00178 int decode_euler_orientation_standard_deviation_packet(euler_orientation_standard_deviation_packet_t *euler_orientation_standard_deviation, an_packet_t *an_packet)
00179 {
00180         if(an_packet->id == packet_id_euler_orientation_standard_deviation && an_packet->length == 12)
00181         {
00182                 memcpy(&euler_orientation_standard_deviation->standard_deviation[0], &an_packet->data[0], 3*sizeof(float));
00183                 return 0;
00184         }
00185         else return 1;
00186 }
00187 
00188 int decode_quaternion_orientation_standard_deviation_packet(quaternion_orientation_standard_deviation_packet_t *quaternion_orientation_standard_deviation_packet, an_packet_t *an_packet)
00189 {
00190         if(an_packet->id == packet_id_quaternion_orientation_standard_deviation && an_packet->length == 16)
00191         {
00192                 memcpy(&quaternion_orientation_standard_deviation_packet->standard_deviation[0], &an_packet->data[0], 4*sizeof(float));
00193                 return 0;
00194         }
00195         else return 1;
00196 }
00197 
00198 int decode_raw_sensors_packet(raw_sensors_packet_t *raw_sensors_packet, an_packet_t *an_packet)
00199 {
00200         if(an_packet->id == packet_id_raw_sensors && an_packet->length == 48)
00201         {
00202                 memcpy(&raw_sensors_packet->accelerometers[0], &an_packet->data[0], 3 * sizeof(float));
00203                 memcpy(&raw_sensors_packet->gyroscopes[0], &an_packet->data[12], 3 * sizeof(float));
00204                 memcpy(&raw_sensors_packet->magnetometers[0], &an_packet->data[24], 3 * sizeof(float));
00205                 memcpy(&raw_sensors_packet->imu_temperature, &an_packet->data[36], sizeof(float));
00206                 return 0;
00207         }
00208         else return 1;
00209 }
00210 
00211 int decode_euler_orientation_packet(euler_orientation_packet_t *euler_orientation_packet, an_packet_t *an_packet)
00212 {
00213         if(an_packet->id == packet_id_euler_orientation && an_packet->length == 12)
00214         {
00215                 memcpy(&euler_orientation_packet->orientation[0], &an_packet->data[0], 3*sizeof(float));
00216                 return 0;
00217         }
00218         else return 1;
00219 }
00220 
00221 int decode_quaternion_orientation_packet(quaternion_orientation_packet_t *quaternion_orientation_packet, an_packet_t *an_packet)
00222 {
00223         if(an_packet->id == packet_id_quaternion_orientation && an_packet->length == 16)
00224         {
00225                 memcpy(&quaternion_orientation_packet->orientation[0], &an_packet->data[0], 4*sizeof(float));
00226                 return 0;
00227         }
00228         else return 1;
00229 }
00230 
00231 int decode_dcm_orientation_packet(dcm_orientation_packet_t *dcm_orientation_packet, an_packet_t *an_packet)
00232 {
00233         if(an_packet->id == packet_id_dcm_orientation && an_packet->length == 36)
00234         {
00235                 memcpy(&dcm_orientation_packet->orientation[0][0], &an_packet->data[0], 9*sizeof(float));
00236                 return 0;
00237         }
00238         else return 1;
00239 }
00240 
00241 int decode_angular_velocity_packet(angular_velocity_packet_t *angular_velocity_packet, an_packet_t *an_packet)
00242 {
00243         if(an_packet->id == packet_id_angular_velocity && an_packet->length == 12)
00244         {
00245                 memcpy(&angular_velocity_packet->angular_velocity[0], &an_packet->data[0], 3*sizeof(float));
00246                 return 0;
00247         }
00248         else return 1;
00249 }
00250 
00251 int decode_angular_acceleration_packet(angular_acceleration_packet_t *angular_acceleration_packet, an_packet_t *an_packet)
00252 {
00253         if(an_packet->id == packet_id_angular_acceleration && an_packet->length == 12)
00254         {
00255                 memcpy(&angular_acceleration_packet->angular_acceleration[0], &an_packet->data[0], 3*sizeof(float));
00256                 return 0;
00257         }
00258         else return 1;
00259 }
00260 
00261 int decode_external_position_velocity_packet(external_position_velocity_packet_t *external_position_velocity_packet, an_packet_t *an_packet)
00262 {
00263         if(an_packet->id == packet_id_external_position_velocity && an_packet->length == 60)
00264         {
00265                 memcpy(&external_position_velocity_packet->position[0], &an_packet->data[0], 3*sizeof(double));
00266                 memcpy(&external_position_velocity_packet->velocity[0], &an_packet->data[24], 3*sizeof(float));
00267                 memcpy(&external_position_velocity_packet->position_standard_deviation[0], &an_packet->data[36], 3*sizeof(float));
00268                 memcpy(&external_position_velocity_packet->velocity_standard_deviation[0], &an_packet->data[48], 3*sizeof(float));
00269                 return 0;
00270         }
00271         else return 1;
00272 }
00273 
00274 an_packet_t *encode_external_position_velocity_packet(external_position_velocity_packet_t *external_position_velocity_packet)
00275 {
00276         an_packet_t *an_packet = an_packet_allocate(60, packet_id_external_position_velocity);
00277         if(an_packet != NULL)
00278         {
00279                 memcpy(&an_packet->data[0], &external_position_velocity_packet->position[0], 3*sizeof(double));
00280                 memcpy(&an_packet->data[24], &external_position_velocity_packet->velocity[0], 3*sizeof(float));
00281                 memcpy(&an_packet->data[36], &external_position_velocity_packet->position_standard_deviation[0], 3*sizeof(float));
00282                 memcpy(&an_packet->data[48], &external_position_velocity_packet->velocity_standard_deviation[0], 3*sizeof(float));
00283         }
00284         return an_packet;
00285 }
00286 
00287 int decode_external_position_packet(external_position_packet_t *external_position_packet, an_packet_t *an_packet)
00288 {
00289         if(an_packet->id == packet_id_external_position && an_packet->length == 36)
00290         {
00291                 memcpy(&external_position_packet->position[0], &an_packet->data[0], 3*sizeof(double));
00292                 memcpy(&external_position_packet->standard_deviation[0], &an_packet->data[24], 3*sizeof(float));
00293                 return 0;
00294         }
00295         else return 1;
00296 }
00297 
00298 an_packet_t *encode_external_position_packet(external_position_packet_t *external_position_packet)
00299 {
00300         an_packet_t *an_packet = an_packet_allocate(36, packet_id_external_position);
00301         if(an_packet != NULL)
00302         {
00303                 memcpy(&an_packet->data[0], &external_position_packet->position[0], 3*sizeof(double));
00304                 memcpy(&an_packet->data[24], &external_position_packet->standard_deviation[0], 3*sizeof(float));
00305         }
00306         return an_packet;
00307 }
00308 
00309 int decode_external_velocity_packet(external_velocity_packet_t *external_velocity_packet, an_packet_t *an_packet)
00310 {
00311         if(an_packet->id == packet_id_external_velocity && an_packet->length == 24)
00312         {
00313                 memcpy(&external_velocity_packet->velocity[0], &an_packet->data[0], 3*sizeof(float));
00314                 memcpy(&external_velocity_packet->standard_deviation[0], &an_packet->data[12], 3*sizeof(float));
00315                 return 0;
00316         }
00317         else return 1;
00318 }
00319 
00320 an_packet_t *encode_external_velocity_packet(external_velocity_packet_t *external_velocity_packet)
00321 {
00322         an_packet_t *an_packet = an_packet_allocate(24, packet_id_external_velocity);
00323         if(an_packet != NULL)
00324         {
00325                 memcpy(&an_packet->data[0], &external_velocity_packet->velocity[0], 3*sizeof(float));
00326                 memcpy(&an_packet->data[12], &external_velocity_packet->standard_deviation[0], 3*sizeof(float));
00327         }
00328         return an_packet;
00329 }
00330 
00331 int decode_external_heading_packet(external_heading_packet_t *external_heading_packet, an_packet_t *an_packet)
00332 {
00333         if(an_packet->id == packet_id_external_heading && an_packet->length == 8)
00334         {
00335                 memcpy(&external_heading_packet->heading, &an_packet->data[0], sizeof(float));
00336                 memcpy(&external_heading_packet->standard_deviation, &an_packet->data[4], sizeof(float));
00337                 return 0;
00338         }
00339         else return 1;
00340 }
00341 
00342 an_packet_t *encode_external_heading_packet(external_heading_packet_t *external_heading_packet)
00343 {
00344         an_packet_t *an_packet = an_packet_allocate(8, packet_id_external_heading);
00345         if(an_packet != NULL)
00346         {
00347                 memcpy(&an_packet->data[0], &external_heading_packet->heading, sizeof(float));
00348                 memcpy(&an_packet->data[4], &external_heading_packet->standard_deviation, sizeof(float));
00349         }
00350         return an_packet;
00351 }
00352 
00353 int decode_running_time_packet(running_time_packet_t *running_time_packet, an_packet_t *an_packet)
00354 {
00355         if(an_packet->id == packet_id_running_time && an_packet->length == 8)
00356         {
00357                 memcpy(&running_time_packet->seconds, &an_packet->data[0], sizeof(uint32_t));
00358                 memcpy(&running_time_packet->microseconds, &an_packet->data[4], sizeof(uint32_t));
00359                 return 0;
00360         }
00361         else return 1;
00362 }
00363 
00364 int decode_local_magnetics_packet(local_magnetics_packet_t *local_magnetics_packet, an_packet_t *an_packet)
00365 {
00366         if(an_packet->id == packet_id_local_magnetics && an_packet->length == 12)
00367         {
00368                 memcpy(&local_magnetics_packet->magnetic_field[0], &an_packet->data[0], 3*sizeof(float));
00369                 return 0;
00370         }
00371         else return 1;
00372 }
00373 
00374 int decode_packet_timer_period_packet(packet_timer_period_packet_t *packet_timer_period_packet, an_packet_t *an_packet)
00375 {
00376     if(an_packet->id == packet_id_packet_timer_period && an_packet->length == 4)
00377     {
00378         packet_timer_period_packet->permanent = an_packet->data[0];
00379         packet_timer_period_packet->utc_synchronisation = an_packet->data[1];
00380         memcpy(&packet_timer_period_packet->packet_timer_period, &an_packet->data[2], sizeof(uint16_t));
00381         return 0;
00382     }
00383     else return 1;
00384 }
00385 
00386 an_packet_t *encode_packet_timer_period_packet(packet_timer_period_packet_t *packet_timer_period_packet)
00387 {
00388         an_packet_t *an_packet = an_packet_allocate(4, packet_id_packet_timer_period);
00389         if(an_packet != NULL)
00390         {
00391                 an_packet->data[0] = packet_timer_period_packet->permanent > 0;
00392                 an_packet->data[1] = packet_timer_period_packet->utc_synchronisation > 0;
00393                 memcpy(&an_packet->data[2], &packet_timer_period_packet->packet_timer_period, sizeof(uint16_t));
00394         }
00395     return an_packet;
00396 }
00397 
00398 int decode_packet_periods_packet(packet_periods_packet_t *packet_periods_packet, an_packet_t *an_packet)
00399 {
00400     if(an_packet->id == packet_id_packet_periods && (an_packet->length - 2) % 5 == 0)
00401     {
00402         int i;
00403         int packet_periods_count = (an_packet->length - 2) / 5;
00404         packet_periods_packet->permanent = an_packet->data[0];
00405         packet_periods_packet->clear_existing_packets = an_packet->data[1];
00406         for(i = 0; i < MAXIMUM_PACKET_PERIODS; i++)
00407         {
00408             if(i < packet_periods_count)
00409             {
00410                 packet_periods_packet->packet_periods[i].packet_id = an_packet->data[2 + 5*i];
00411                 memcpy(&packet_periods_packet->packet_periods[i].period, &an_packet->data[2 + 5*i + 1], sizeof(uint32_t));
00412             }
00413             else memset(&packet_periods_packet->packet_periods[i], 0, sizeof(packet_period_t));
00414         }
00415         return 0;
00416     }
00417     else return 1;
00418 }
00419 
00420 an_packet_t *encode_packet_periods_packet(packet_periods_packet_t *packet_periods_packet)
00421 {
00422     int i;
00423     an_packet_t *an_packet = an_packet_allocate(252, packet_id_packet_periods);
00424         if(an_packet != NULL)
00425         {
00426                 an_packet->data[0] = packet_periods_packet->permanent > 0;
00427                 an_packet->data[1] = packet_periods_packet->clear_existing_packets;
00428                 for(i = 0; i < MAXIMUM_PACKET_PERIODS; i++)
00429                 {
00430                         if(packet_periods_packet->packet_periods[i].packet_id)
00431                         {
00432                                 an_packet->data[2 + 5*i] = packet_periods_packet->packet_periods[i].packet_id;
00433                                 memcpy(&an_packet->data[2 + 5*i + 1], &packet_periods_packet->packet_periods[i].period, sizeof(uint32_t));
00434                         }
00435                         else break;
00436                 }
00437                 an_packet->length = 2 + 5*i;
00438         }
00439     return an_packet;
00440 }
00441 
00442 int decode_baud_rates_packet(baud_rates_packet_t *baud_rates_packet, an_packet_t *an_packet)
00443 {
00444         if(an_packet->id == packet_id_baud_rates && an_packet->length == 17)
00445         {
00446                 baud_rates_packet->permanent = an_packet->data[0];
00447                 memcpy(&baud_rates_packet->primary_baud_rate, &an_packet->data[1], sizeof(uint32_t));
00448                 memcpy(&baud_rates_packet->gpio_1_2_baud_rate, &an_packet->data[5], sizeof(uint32_t));
00449                 memcpy(&baud_rates_packet->gpio_3_4_baud_rate, &an_packet->data[9], sizeof(uint32_t));
00450                 return 0;
00451         }
00452         else return 1;
00453 }
00454 
00455 an_packet_t *encode_baud_rates_packet(baud_rates_packet_t *baud_rates_packet)
00456 {
00457         an_packet_t *an_packet = an_packet_allocate(17, packet_id_baud_rates);
00458         if(an_packet != NULL)
00459         {
00460                 an_packet->data[0] = baud_rates_packet->permanent;
00461                 memcpy(&an_packet->data[1], &baud_rates_packet->primary_baud_rate, sizeof(uint32_t));
00462                 memcpy(&an_packet->data[5], &baud_rates_packet->gpio_1_2_baud_rate, sizeof(uint32_t));
00463                 memcpy(&an_packet->data[9], &baud_rates_packet->gpio_3_4_baud_rate, sizeof(uint32_t));
00464                 memset(&an_packet->data[13], 0, sizeof(uint32_t));
00465         }
00466         return an_packet;
00467 }
00468 
00469 int decode_sensor_ranges_packet(sensor_ranges_packet_t *sensor_ranges_packet, an_packet_t *an_packet)
00470 {
00471         if(an_packet->id == packet_id_sensor_ranges && an_packet->length == 4)
00472         {
00473                 memcpy(sensor_ranges_packet, an_packet->data, 4*sizeof(uint8_t));
00474                 return 0;
00475         }
00476         else return 1;
00477 }
00478 
00479 an_packet_t *encode_sensor_ranges_packet(sensor_ranges_packet_t *sensor_ranges_packet)
00480 {
00481         an_packet_t *an_packet = an_packet_allocate(4, packet_id_sensor_ranges);
00482         if(an_packet != NULL)
00483         {
00484                 memcpy(an_packet->data, sensor_ranges_packet, 4*sizeof(uint8_t));
00485         }
00486         return an_packet;
00487 }
00488 
00489 int decode_installation_alignment_packet(installation_alignment_packet_t *installation_alignment_packet, an_packet_t *an_packet)
00490 {
00491         if(an_packet->id == packet_id_installation_alignment && an_packet->length == 73)
00492         {
00493                 installation_alignment_packet->permanent = an_packet->data[0];
00494                 memcpy(&installation_alignment_packet->alignment_dcm[0][0], &an_packet->data[1], 9*sizeof(float));
00495                 return 0;
00496         }
00497         else return 1;
00498 }
00499 
00500 an_packet_t *encode_installation_alignment_packet(installation_alignment_packet_t *installation_alignment_packet)
00501 {
00502         an_packet_t *an_packet = an_packet_allocate(73, packet_id_installation_alignment);
00503         if(an_packet != NULL)
00504         {
00505                 an_packet->data[0] = installation_alignment_packet->permanent;
00506                 memcpy(&an_packet->data[1], &installation_alignment_packet->alignment_dcm[0][0], 9*sizeof(float));
00507                 memset(&an_packet->data[37], 0, 9*sizeof(float));
00508         }
00509         return an_packet;
00510 }
00511 
00512 int decode_filter_options_packet(filter_options_packet_t *filter_options_packet, an_packet_t *an_packet)
00513 {
00514         if(an_packet->id == packet_id_filter_options && an_packet->length == 17)
00515         {
00516                 filter_options_packet->permanent = an_packet->data[0];
00517                 filter_options_packet->vehicle_type = an_packet->data[1];
00518                 filter_options_packet->magnetometers_enabled = an_packet->data[3];
00519                 filter_options_packet->velocity_heading_enabled = an_packet->data[5];
00520                 return 0;
00521         }
00522         else return 1;
00523 }
00524 
00525 an_packet_t *encode_filter_options_packet(filter_options_packet_t *filter_options_packet)
00526 {
00527         an_packet_t *an_packet = an_packet_allocate(17, packet_id_filter_options);
00528         if(an_packet != NULL)
00529         {
00530                 memset(an_packet->data, 0, 17*sizeof(uint8_t));
00531                 an_packet->data[0] = filter_options_packet->permanent;
00532                 an_packet->data[1] = filter_options_packet->vehicle_type;
00533                 an_packet->data[3] = filter_options_packet->magnetometers_enabled;
00534                 an_packet->data[5] = filter_options_packet->velocity_heading_enabled;
00535         }
00536         return an_packet;
00537 }
00538 
00539 int decode_gpio_configuration_packet(gpio_configuration_packet_t *gpio_configuration_packet, an_packet_t *an_packet)
00540 {
00541         if(an_packet->id == packet_id_gpio_configuration && an_packet->length == 13)
00542         {
00543                 memcpy(gpio_configuration_packet, &an_packet->data[0], 5*sizeof(uint8_t));
00544                 return 0;
00545         }
00546         else return 1;
00547 }
00548 
00549 an_packet_t *encode_gpio_configuration_packet(gpio_configuration_packet_t *gpio_configuration_packet)
00550 {
00551         an_packet_t *an_packet = an_packet_allocate(13, packet_id_gpio_configuration);
00552         if(an_packet != NULL)
00553         {
00554                 memcpy(&an_packet->data[0], gpio_configuration_packet, 5*sizeof(uint8_t));
00555                 memset(&an_packet->data[5], 0, 8*sizeof(uint8_t));
00556         }
00557         return an_packet;
00558 }
00559 
00560 int decode_magnetic_calibration_values_packet(magnetic_calibration_values_packet_t *magnetic_calibration_values_packet, an_packet_t *an_packet)
00561 {
00562         if(an_packet->id == packet_id_magnetic_calibration_values && an_packet->length == 49)
00563         {
00564                 magnetic_calibration_values_packet->permanent = an_packet->data[0];
00565                 memcpy(magnetic_calibration_values_packet->hard_iron, &an_packet->data[5], 3*sizeof(float));
00566                 memcpy(magnetic_calibration_values_packet->soft_iron, &an_packet->data[13], 9*sizeof(float));
00567                 return 0;
00568         }
00569         else return 1;
00570 }
00571 
00572 an_packet_t *encode_magnetic_calibration_values_packet(magnetic_calibration_values_packet_t *magnetic_calibration_values_packet)
00573 {
00574         an_packet_t *an_packet = an_packet_allocate(49, packet_id_magnetic_calibration_values);
00575         if(an_packet != NULL)
00576         {
00577                 an_packet->data[0] = magnetic_calibration_values_packet->permanent;
00578                 memcpy(&an_packet->data[1], magnetic_calibration_values_packet->hard_iron, 3*sizeof(float));
00579                 memcpy(&an_packet->data[13], magnetic_calibration_values_packet->soft_iron, 9*sizeof(float));
00580         }
00581         return an_packet;
00582 }
00583 
00584 an_packet_t *encode_magnetic_calibration_configuration_packet(magnetic_calibration_configuration_packet_t *magnetic_calibration_configuration_packet)
00585 {
00586         an_packet_t *an_packet = an_packet_allocate(1, packet_id_magnetic_calibration_configuration);
00587         if(an_packet != NULL)
00588         {
00589                 an_packet->data[0] = magnetic_calibration_configuration_packet->magnetic_calibration_action;
00590         }
00591         return an_packet;
00592 }
00593 
00594 int decode_magnetic_calibration_status_packet(magnetic_calibration_status_packet_t *magnetic_calibration_status_packet, an_packet_t *an_packet)
00595 {
00596         if(an_packet->id == packet_id_magnetic_calibration_status && an_packet->length == 3)
00597         {
00598                 magnetic_calibration_status_packet->magnetic_calibration_status = an_packet->data[0];
00599                 magnetic_calibration_status_packet->magnetic_calibration_progress_percentage = an_packet->data[1];
00600                 magnetic_calibration_status_packet->local_magnetic_error_percentage = an_packet->data[2];
00601                 return 0;
00602         }
00603         return 1;
00604 }
00605 
00606 an_packet_t *encode_zero_alignment_packet(zero_alignment_packet_t *zero_alignment_packet)
00607 {
00608         an_packet_t *an_packet = an_packet_allocate(1, packet_id_zero_alignment);
00609         if(an_packet != NULL)
00610         {
00611                 an_packet->data[0] = zero_alignment_packet->permanent;
00612         }
00613         return an_packet;
00614 }
00615 
00616 
00617 
00618 
00619 int decode_acceleration_packet(acceleration_packet_t *acceleration_packet, an_packet_t *an_packet)
00620 {
00621   if(an_packet->id == packet_id_acceleration && an_packet->length == 12)
00622     {
00623       memcpy(&acceleration_packet->acceleration[0], &an_packet->data[0], 3*sizeof(float));
00624       return 0;
00625 
00626     }
00627   else return 1;
00628 }


orientus_sdk_c
Author(s): Advanced Navigation, Nick Otero
autogenerated on Wed Aug 26 2015 15:12:17