packet_example.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 <stdlib.h>
00031 #include <stdio.h>
00032 #include <stdint.h>
00033 #include <math.h>
00034 
00035 #ifdef _WIN32
00036 #include <Windows.h>
00037 #else
00038 #include <unistd.h>
00039 #endif
00040 
00041 #include "rs232/rs232.h"
00042 #include "an_packet_protocol.h"
00043 #include "orientus_packets.h"
00044 
00045 #define RADIANS_TO_DEGREES (180.0/M_PI)
00046 
00047 int com_port_number;
00048 
00049 int an_packet_transmit(an_packet_t *an_packet)
00050 {
00051         an_packet_encode(an_packet);
00052         return SendBuf(com_port_number, an_packet_pointer(an_packet), an_packet_size(an_packet));
00053 }
00054 
00055 /*
00056  * This is an example of sending a configuration packet to Orientus.
00057  *
00058  * 1. First declare the structure for the packet, in this case sensor_ranges_packet_t.
00059  * 2. Set all the fields of the packet structure
00060  * 3. Encode the packet structure into an an_packet_t using the appropriate helper function
00061  * 4. Send the packet
00062  * 5. Free the packet
00063  */
00064 void set_sensor_ranges()
00065 {
00066         an_packet_t *an_packet;
00067         sensor_ranges_packet_t sensor_ranges_packet;
00068 
00069         sensor_ranges_packet.permanent = TRUE;
00070         sensor_ranges_packet.accelerometers_range = accelerometer_range_4g;
00071         sensor_ranges_packet.gyroscopes_range = gyroscope_range_500dps;
00072         sensor_ranges_packet.magnetometers_range = magnetometer_range_2g;
00073 
00074         an_packet = encode_sensor_ranges_packet(&sensor_ranges_packet);
00075 
00076         an_packet_transmit(an_packet);
00077 
00078         an_packet_free(&an_packet);
00079 }
00080 
00081 int main(int argc, char *argv[])
00082 {
00083         an_decoder_t an_decoder;
00084         an_packet_t *an_packet;
00085         
00086         status_packet_t status_packet;
00087         euler_orientation_packet_t euler_orientation_packet;
00088         raw_sensors_packet_t raw_sensors_packet;
00089         
00090         int bytes_received;
00091 
00092         if (argc != 3)
00093         {
00094                 printf("Usage - program com_port_number baud_rate\nExample - packet_example.exe 0 115200\n");
00095                 exit(EXIT_FAILURE);
00096         }
00097 
00098         com_port_number = atoi(argv[1]);
00099         
00100         /* open the com port */
00101         if (OpenComport(com_port_number, atoi(argv[2])))
00102         {
00103                 exit(EXIT_FAILURE);
00104         }
00105         
00106         an_decoder_initialise(&an_decoder);
00107 
00108         while (1)
00109         {
00110                 if ((bytes_received = PollComport(com_port_number, an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
00111                 {
00112                         /* increment the decode buffer length by the number of bytes received */
00113                         an_decoder_increment(&an_decoder, bytes_received);
00114                         
00115                         /* decode all the packets in the buffer */
00116                         while ((an_packet = an_packet_decode(&an_decoder)) != NULL)
00117                         {
00118                                 if(an_packet->id == packet_id_status)
00119                                 {
00120                                         if(decode_status_packet(&status_packet, an_packet) == 0)
00121                                         {
00122                                                 printf("Status Packet:\n");
00123                                                 printf("\tFilter Ready = %d\n", status_packet.filter_status.b.orientation_filter_initialised);
00124                                         }
00125                                 }
00126                                 else if (an_packet->id == packet_id_euler_orientation) /* system state packet */
00127                                 {
00128                                         /* copy all the binary data into the typedef struct for the packet */
00129                                         /* this allows easy access to all the different values             */
00130                                         if(decode_euler_orientation_packet(&euler_orientation_packet, an_packet) == 0)
00131                                         {
00132                                                 printf("Euler Orientation Packet:\n");
00133                                                 printf("\tRoll = %f, Pitch = %f, Yaw = %f\n", euler_orientation_packet.orientation[0] * RADIANS_TO_DEGREES, euler_orientation_packet.orientation[1] * RADIANS_TO_DEGREES, euler_orientation_packet.orientation[2] * RADIANS_TO_DEGREES);
00134                                         }
00135                                 }
00136                                 else if (an_packet->id == packet_id_raw_sensors) /* raw sensors packet */
00137                                 {
00138                                         /* copy all the binary data into the typedef struct for the packet */
00139                                         /* this allows easy access to all the different values             */
00140                                         if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0)
00141                                         {
00142                                                 printf("Raw Sensors Packet:\n");
00143                                                 printf("\tAccelerometers X: %f Y: %f Z: %f\n", raw_sensors_packet.accelerometers[0], raw_sensors_packet.accelerometers[1], raw_sensors_packet.accelerometers[2]);
00144                                                 printf("\tGyroscopes X: %f Y: %f Z: %f\n", raw_sensors_packet.gyroscopes[0] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[1] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[2] * RADIANS_TO_DEGREES);
00145                                         }
00146                                 }
00147                                 else
00148                                 {
00149                                         printf("Packet ID %u of Length %u\n", an_packet->id, an_packet->length);
00150                                 }
00151                                 
00152                                 /* Ensure that you free the an_packet when your done with it or you will leak memory */
00153                                 an_packet_free(&an_packet);
00154                         }
00155                 }
00156 #ifdef _WIN32
00157     Sleep(10);
00158 #else
00159     usleep(10000);
00160 #endif
00161         }
00162 }


orientus_sdk_c
Author(s): Advanced Navigation, Nick Otero
autogenerated on Thu Jun 6 2019 20:17:59