sdk.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2011, Ascending Technologies GmbH
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 
00033 #ifndef SDK_
00034 #define SDK_
00035 
00036 void SDK_mainloop(void);
00037 
00038 #include <HL_interface.h>
00039 
00040 #define CMD_MAX_PERIOD 100 // in [ms] ==> 10Hz
00041 
00043 void sdkInit(void);
00044 
00046 inline void writeCommand(short pitch, short roll, short yaw, short thrust, short ctrl, short enable);
00047 
00049 inline void sendImuData(void);
00050 
00052 inline void sendGpsData(void);
00053 
00055 inline void sendStatus(void);
00056 
00058 inline void sendRcData(void);
00059 
00061 inline void sendMagData(void);
00062 
00064 /***
00065  * Timestamped packets get send around every 2 s to average the transmission delay.
00066  * Corrects at max 500 us per second. If the time offset is large, the server (host PC)
00067  * time is taken directly and synchronization starts from that time.
00068  */
00069 inline void synchronizeTime(void);
00070 
00072 inline void watchdog(void);
00073 
00075 inline int checkTxPeriod(uint16_t period);
00076 
00078 inline void predictEkfState(void);
00079 
00081 inline void sendEkfState(void);
00082 
00084 extern unsigned int sdkLoops;
00085 
00086 extern HLI_EXT_POSITION extPosition;
00087 extern HLI_CMD_HL extPositionCmd;
00088 extern HLI_STATUS statusData;
00089 
00090 extern HLI_CONFIG hli_config;
00091 
00093 extern volatile int64_t timestamp;
00094 
00095 
00096 //--- general commands -----------------------------------------------------------------------------------------------------------------------------------------------
00097 struct WO_SDK_STRUCT {
00098 
00099         unsigned char ctrl_mode;
00100 
00101         unsigned char ctrl_enabled; //0x00: control commands are ignored by LL processor
00102                                                                 //0x01: control commands are accepted by LL processor
00103 
00104         unsigned char disable_motor_onoff_by_stick; // if true, "minimum thrust + full yaw" command will not start/stop motors
00105 };
00106 extern struct WO_SDK_STRUCT WO_SDK;
00107 
00108 //--- read sensor data -----------------------------------------------------------------------------------------------------------------------------------------------
00109 struct RO_ALL_DATA {
00110 
00111         //remote control data
00112                 unsigned short channel[8];
00113                 /*
00114                  * channel[0]: pitch
00115                  * channel[1]: roll
00116                  * channel[2]: thrust
00117                  * channel[3]: yaw
00118                  * channel[4]: serial interface enable/disable (>2048 enabled)
00119                  * channel[5]: manual / height control / GPS + height control (0 -> manual mode; 2048 -> height mode; 4095 -> GPS mode)
00120                  *
00121                  * range of each channel: 0..4095
00122                  */
00123 
00124         //angles derived by integration of gyro_outputs, drift compensated by data fusion; -90000..+90000 pitch(nick) and roll, 0..360000 yaw; 1000 = 1 degree
00125             int angle_pitch;
00126             int angle_roll;
00127             int angle_yaw;
00128 
00129         //angular velocities, bias free, in 0.0154 �/s (=> 64.8 = 1 �/s)
00130             int angvel_pitch;
00131             int angvel_roll;
00132             int angvel_yaw;
00133 
00134         //acc-sensor outputs, calibrated: -10000..+10000 = -1g..+1g, body frame coordinate system
00135             short acc_x;
00136             short acc_y;
00137             short acc_z;
00138 
00139         //magnetic field sensors output, offset free and scaled to +-2500 = +- earth field strength;
00140             int Hx;
00141             int Hy;
00142             int Hz;
00143 
00144         //RPM measurements (0..200)
00145         /*
00146          * Quadcopter (AscTec Hummingbird, AscTec Pelican)
00147          *
00148          * motor[0]: front
00149          * motor[1]: rear
00150          * motor[2]: left
00151          * motor[3]: right
00152          *
00153          */
00154 
00155         /*
00156          * Hexcopter (AscTec Firefly)
00157          *
00158          * motor[0]: front-left
00159          * motor[1]: left
00160          * motor[2]: rear-left
00161          * motor[3]: rear-right
00162          * motor[4]: right
00163          * motor[5]: front-right
00164          *
00165          */
00166             unsigned char motor_rpm[6];
00167 
00168         //latitude/longitude in degrees * 10^7
00169                 int GPS_latitude;
00170                 int GPS_longitude;
00171 
00172         //GPS height in mm
00173                 int GPS_height;
00174 
00175         //speed in x (E/W) and y(N/S) in mm/s
00176                 int GPS_speed_x;
00177                 int GPS_speed_y;
00178 
00179         //GPS heading in deg * 1000
00180                 int GPS_heading;
00181 
00182         //accuracy estimates in mm and mm/s
00183                 unsigned int GPS_position_accuracy;
00184                 unsigned int GPS_height_accuracy;
00185                 unsigned int GPS_speed_accuracy;
00186 
00187         //number of satellites used in NAV solution
00188                 unsigned int GPS_sat_num;
00189 
00190         // GPS status information: Bit7...Bit3: 0; Bit 2: longitude direction; Bit1: latitude direction; Bit 0: GPS lock
00191                 int GPS_status;
00192 
00193                 unsigned int GPS_time_of_week;  //[ms] (1 week = 604,800 s)
00194                 unsigned short GPS_week;                // starts from beginning of year 1980
00195 
00196         //height in mm (after data fusion)
00197                 int fusion_height;
00198 
00199         //diff. height in mm/s (after data fusion)
00200                 int fusion_dheight;
00201 
00202         //GPS data fused with all other sensors (best estimations)
00203                 int fusion_latitude;    //Fused latitude in degrees * 10^7
00204                 int fusion_longitude;   //Fused longitude in degrees * 10^7
00205 
00206                 short fusion_speed_x;   //[mm/s]
00207                 short fusion_speed_y;   //[mm/s]
00208 
00209 }; //************************************************************
00210 extern struct RO_ALL_DATA RO_ALL_Data;
00211 
00212 
00213 struct RO_RC_DATA {
00214 
00215         unsigned short channel[8];
00216         /*
00217          * channel[0]: pitch
00218          * channel[1]: roll
00219          * channel[2]: thrust
00220          * channel[3]: yaw
00221          * channel[4]: serial interface enable/disable (>2048 enabled)
00222          * channel[5]: manual / height control / GPS + height control (0-manual-1500-height-2500-GPS-4095)
00223          *
00224          * range of each channel: 0..4095
00225          */
00226 };
00227 extern struct RO_RC_DATA RO_RC_Data;
00228 
00229 
00230 
00231 //--- send commands -----------------------------------------------------------------------------------------------------------------------------------------------
00232 struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL
00233 {
00234         unsigned char motor[8];
00235 
00236         /*
00237          * commands will be directly interpreted by each motor individually
00238          *
00239          * range: 0..200 = 0..100 %; 0 = motor off! Please check for command != 0 during flight, as a motor restart might take > 1s!
00240          */
00241 
00242         /*
00243          * Quadcopter (AscTec Hummingbird, AscTec Pelican)
00244          *
00245          * motor[0]: front
00246          * motor[1]: rear
00247          * motor[2]: left
00248          * motor[3]: right
00249          *
00250          */
00251 
00252         /*
00253          * Hexcopter (AscTec Firefly)
00254          *
00255          * motor[0]: front-left
00256          * motor[1]: left
00257          * motor[2]: rear-left
00258          * motor[3]: rear-right
00259          * motor[4]: right
00260          * motor[5]: front-right
00261          *
00262          */
00263 
00264 };
00265 extern struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL WO_Direct_Individual_Motor_Control;
00266 
00267 struct WO_DIRECT_MOTOR_CONTROL //direct motor commands with standard output mapping
00268 {
00269         unsigned char pitch;
00270         unsigned char roll;
00271         unsigned char yaw;
00272         unsigned char thrust;
00273 
00274         /*
00275          * commands will be directly interpreted by the mixer
00276          * running on each of the motor controllers
00277          *
00278          * range (pitch, roll, yaw commands): 0..200 = - 100..+100 %
00279          * range of thrust command: 0..200 = 0..100 %
00280          */
00281 
00282 };
00283 extern struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control;
00284 
00285 
00286 //attitude commands
00287 
00288 struct WO_CTRL_INPUT {
00289 
00290         short pitch;    //pitch input: -2047..+2047 (0=neutral)
00291         short roll;             //roll input: -2047..+2047      (0=neutral)
00292         short yaw;              //(=R/C Stick input) -2047..+2047 (0=neutral)
00293         short thrust;   //collective: 0..4095 = 0..100%
00294         short ctrl;                             /*control byte:
00295                                                         bit 0: pitch control enabled
00296                                                         bit 1: roll control enabled
00297                                                         bit 2: yaw control enabled
00298                                                         bit 3: thrust control enabled
00299                                                         bit 4: height control enabled
00300                                                         bit 5: GPS position control enabled
00301                                                         */
00302 
00303         //max. pitch/roll (+-2047) equals 51.2� angle if GPS position control is disabled
00304         //max. pitch/roll (+-2047) equals approx. 3 m/s if GPS position control is active (GPS signal needed!)
00305 
00306 };
00307 extern struct WO_CTRL_INPUT WO_CTRL_Input;
00308 
00309 
00310 //waypoint commands
00311 
00312 struct WAYPOINT { //waypoint definition
00313 //always set to 1
00314   unsigned int wp_activated;
00315 
00316 //use WPPROP_* defines to set waypoint properties
00317   unsigned char properties;
00318 
00319 //max. speed to travel to waypoint in % (default 100)
00320   unsigned char max_speed;
00321 
00322 //time to stay at a waypoint (XYZ) in 1/100 s
00323   unsigned short time;
00324 
00325 //position accuracy to consider a waypoint reached in mm (recommended: 3000 (= 3.0 m))
00326   unsigned short pos_acc;
00327 
00328 //chksum = 0xAAAA + wp.yaw + wp.height + wp.time + wp.X + wp.Y + wp.max_speed + wp.pos_acc + wp.properties + wp.wp_number;
00329   short chksum;
00330 
00331  //relative waypoint coordinates in mm  // longitude in abs coords e.g. 113647430 (= 11.3647430�; angle in degrees * 10^7)
00332   int X;
00333  //relative waypoint coordinates in mm          // latitude in abs coords e.g. 480950480 (= 48.0950480�; angle in degrees * 10^7)
00334   int Y;
00335 
00336 //yaw angle
00337   int yaw; // 1/1000�
00338 
00339 //height over 0 reference in mm
00340   int height;
00341 
00342 };
00343 extern struct WAYPOINT wpToLL;
00344 
00345 //set waypoint properties with WPPROP_* defines (wpToLL.properties)
00346 #define WPPROP_ABSCOORDS                        0x01    //if set waypoint is interpreted as absolute coordinates, else relative coordinates
00347 #define WPPROP_HEIGHTENABLED            0x02    //set new height at waypoint
00348 #define WPPROP_YAWENABLED                       0x04    //set new yaw-angle at waypoint
00349 #define WPPROP_AUTOMATICGOTO            0x10    //if set, vehicle will not wait for a goto command, but goto this waypoint directly
00350 
00351 extern unsigned char wpCtrlWpCmd; //choose actual waypoint command from WP_CMD_* defines
00352 #define WP_CMD_SINGLE_WP        0x01 //fly to single waypoint
00353 #define WP_CMD_LAUNCH           0x02 //launch to 10m at current position
00354 #define WP_CMD_LAND                     0x03 //land at current position
00355 #define WP_CMD_GOHOME           0x04 //come home at current height. Home position is set when motors are started (valid GPS signal mandatory!) or with WP_CMD_SETHOME
00356 #define WP_CMD_SETHOME          0x05 //save current vehicle position as home position
00357 #define WP_CMD_ABORT            0x06 //abort navigation (stops current waypoint flying)
00358 
00359 extern unsigned char wpCtrlWpCmdUpdated; //send current waypoint command to LL
00360 extern unsigned char wpCtrlAckTrigger; //acknowledge from LL processor that waypoint was accepted
00361 
00362 //Waypoint navigation status
00363 extern unsigned short wpCtrlNavStatus; //check navigation status with WP_NAVSTAT_* defines
00364 #define WP_NAVSTAT_REACHED_POS                  0x01    //vehicle has entered a radius of WAYPOINT.pos_acc and time to stay is not necessarily over
00365 #define WP_NAVSTAT_REACHED_POS_TIME             0x02    //vehicle is within a radius of WAYPOINT.pos_acc and time to stay is over
00366 #define WP_NAVSTAT_20M                                  0x04    //vehicle within a 20m radius of the waypoint
00367 #define WP_NAVSTAT_PILOT_ABORT                  0x08    //waypoint navigation aborted by safety pilot (any stick was moved)
00368 
00369 extern unsigned short wpCtrlDistToWp; //current distance to the current waypoint in dm (=10 cm)
00370 
00371 #endif /*SDK_*/


asctec_hl_firmware
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Dec 17 2013 11:39:27