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_*/