| 1 | // %flair:license{
|
---|
| 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
| 4 | // %flair:license}
|
---|
| 5 | /*!
|
---|
| 6 | * \file ParrotNavBoard.h
|
---|
| 7 | * \brief Class for ArDrone 2 sensors
|
---|
| 8 | * \author Guillaume Sanahuja, Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 9 | * \date 2013/12/19
|
---|
| 10 | * \version 4.0
|
---|
| 11 | */
|
---|
| 12 |
|
---|
| 13 | //thanks to Paparazzi (https://wiki.paparazziuav.org) for the struct nav_struct
|
---|
| 14 |
|
---|
| 15 | #ifndef PARROTNAVBOARD_H
|
---|
| 16 | #define PARROTNAVBOARD_H
|
---|
| 17 |
|
---|
| 18 | #include <Thread.h>
|
---|
| 19 |
|
---|
| 20 | namespace flair {
|
---|
| 21 | namespace core {
|
---|
| 22 | class FrameworkManager;
|
---|
| 23 | class cvmatrix;
|
---|
| 24 | class ImuData;
|
---|
| 25 | class SerialPort;
|
---|
| 26 | }
|
---|
| 27 | }
|
---|
| 28 |
|
---|
| 29 | namespace flair { namespace sensor {
|
---|
| 30 | class Imu;
|
---|
| 31 | class UsRangeFinder;
|
---|
| 32 |
|
---|
| 33 | /*! \class ParrotNavBoard
|
---|
| 34 | *
|
---|
| 35 | * \brief Class for ArDrone 2 sensors
|
---|
| 36 | */
|
---|
| 37 | class ParrotNavBoard : public core::Thread {
|
---|
| 38 | public:
|
---|
| 39 | /*!
|
---|
| 40 | * \brief Constructor
|
---|
| 41 | *
|
---|
| 42 | * Construct a ParrotNavBoard.
|
---|
| 43 | *
|
---|
| 44 | * \param parent parent
|
---|
| 45 | * \param name name
|
---|
| 46 | * \param serialport serialport
|
---|
| 47 | * \param priority priority of the Thread
|
---|
| 48 | */
|
---|
| 49 | ParrotNavBoard(const core::FrameworkManager* parent,std::string name,core::SerialPort* serialport,uint8_t priority);
|
---|
| 50 |
|
---|
| 51 | /*!
|
---|
| 52 | * \brief Destructor
|
---|
| 53 | *
|
---|
| 54 | */
|
---|
| 55 | ~ParrotNavBoard();
|
---|
| 56 |
|
---|
| 57 | /*!
|
---|
| 58 | * \brief Use default plot
|
---|
| 59 | *
|
---|
| 60 | */
|
---|
| 61 | void UseDefaultPlot(void);
|
---|
| 62 |
|
---|
| 63 | /*!
|
---|
| 64 | * \brief Get Imu
|
---|
| 65 | *
|
---|
| 66 | * \return Associated Imu
|
---|
| 67 | */
|
---|
| 68 | Imu* GetImu(void) const;
|
---|
| 69 |
|
---|
| 70 | /*!
|
---|
| 71 | * \brief Get UsRangeFinder
|
---|
| 72 | *
|
---|
| 73 | * \return Associated UsRangeFinder
|
---|
| 74 | */
|
---|
| 75 | UsRangeFinder* GetUsRangeFinder(void) const;
|
---|
| 76 |
|
---|
| 77 | private:
|
---|
| 78 | typedef struct nav_struct
|
---|
| 79 | {
|
---|
| 80 | uint16_t size; // +0x00 Size of the following data (always 0x3a)
|
---|
| 81 | uint16_t seq; // +0x02 Sequence number, increases every update
|
---|
| 82 | uint16_t acc[3]; // +0x04 Raw data (10-bit) of the accelerometers multiplied by 4
|
---|
| 83 | int16_t gyro[3]; // +0x0A Raw data for the gyros, 12-bit A/D converted voltage of the gyros. X,Y=IDG, Z=Epson
|
---|
| 84 | uint16_t acc_temp; // +0x10 Accs temperature -- startup value 120 @ 25C, rising to 143
|
---|
| 85 | uint16_t gyro_temp; // +0x12 XYGyro temperature (IDG), 12-bit A/D converted voltage of IDG's temperature sensor -- startup value 1532 @ 25C, rising to 1572
|
---|
| 86 | uint16_t us_echo; // +0x14 bit15=1 echo pulse transmitted, bit14-0 first echo. Value 30 = 1cm. min value: 784 = 26cm
|
---|
| 87 | uint16_t us_echo_start; // +0x16 Array with starts of echos (8 array values @ 25Hz, 9 values @ 22.22Hz)
|
---|
| 88 | uint16_t us_echo_end; // +0x18 Array with ends of echos (8 array values @ 25Hz, 9 values @ 22.22Hz)
|
---|
| 89 | uint16_t us_association_echo; // +0x1a Ultrasonic parameter -- echo number starting with 0. max value 3758. examples: 0,1,2,3,4,5,6,7 ; 0,1,2,3,4,86,6,9
|
---|
| 90 | uint16_t us_distance_echo; // +0x1c Ultrasonic parameter -- no clear pattern
|
---|
| 91 | uint16_t us_courbe_temps; // +0x1e Ultrasonic parameter -- counts up from 0 to approx 24346 in 192 sample cycles of which 12 cylces have value 0
|
---|
| 92 | uint16_t us_courbe_valeur; // +0x20 Ultrasonic parameter -- value between 0 and 4000, no clear pattern. 192 sample cycles of which 12 cylces have value 0
|
---|
| 93 | uint16_t us_courbe_ref; // +0x22 Ultrasonic parameter -- coutns down from 4000 to 0 in 192 sample cycles of which 12 cylces have value 0
|
---|
| 94 | uint16_t nb_echo;
|
---|
| 95 | uint32_t sum_echo;
|
---|
| 96 | int16_t gradient;
|
---|
| 97 | uint16_t flag_echo_ini;
|
---|
| 98 | int32_t pressure;
|
---|
| 99 | int16_t temprature_pressure;
|
---|
| 100 | int16_t mag[3];
|
---|
| 101 | uint16_t checksum;
|
---|
| 102 | } __attribute__ ((packed)) nav_struct;
|
---|
| 103 |
|
---|
| 104 | /*!
|
---|
| 105 | * \brief Run function
|
---|
| 106 | *
|
---|
| 107 | * Reimplemented from Thread.
|
---|
| 108 | *
|
---|
| 109 | */
|
---|
| 110 | void Run(void);
|
---|
| 111 |
|
---|
| 112 | uint16_t Checksum(const uint8_t *data) const;
|
---|
| 113 | void computeDatas(nav_struct* nav,core::Time time);
|
---|
| 114 |
|
---|
| 115 | Imu* imu;
|
---|
| 116 | UsRangeFinder* us;
|
---|
| 117 | core::SerialPort* serialport;
|
---|
| 118 | };
|
---|
| 119 | } // end namespace sensor
|
---|
| 120 | } // end namespace flair
|
---|
| 121 |
|
---|
| 122 | #endif // PARROTNAVBOARD_H
|
---|