00001 /* 00002 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 #ifndef SDK_H 00033 #define SDK_H 00034 00035 #include <inttypes.h> 00036 00037 #include "pid.h" 00038 #include "uart.h" 00039 #include "hardware.h" 00040 #include "kalman.h" 00041 #include "comm_util_LL.h" 00042 00043 #define CMD_MAX_PERIOD 100 // in [ms] ==> 10Hz 00044 00045 #define LAND_THRUST_DECREASE_STEP 0.01 // while landing, decrease thrust by this much 00046 #define LAND_THRUST_DECREASE_PERIOD 1 // while landing, decrease thrust every # of cycles 00047 00048 typedef struct 00049 { 00050 Position x; // x position, in m 00051 Position y; // y position, in m 00052 Position z; // z position, in m 00053 00054 Velocity vx; // x velocity, in m/sec 00055 Velocity vy; // y velocity, in m/sec 00056 Velocity vz; // y velocity, in m/sec 00057 00058 Angle roll; // roll orientation, in rad 00059 Angle pitch; // pitch orientation, in rad 00060 Angle yaw; // yaw orientation, in rad 00061 } MAV_POSE_SI; 00062 00063 typedef struct 00064 { 00065 Angle cmd_roll; // roll , in rad/.... 00066 Angle cmd_pitch; // pitch , in rad 00067 AngVelocity cmd_yaw_rate; // yaw_rate , in rad/s 00068 Thrust cmd_thrust; // thrust, in % [0, 100] 00069 00070 } MAV_CTRL_CMD; 00071 00072 typedef struct 00073 { 00074 Position x; // desired x position, in mm 00075 Position y; // desired y position, in mm 00076 Position z; // desired z position, in mm 00077 00078 Velocity vx; // desired x velocity, in mm/sec 00079 Velocity vy; // desired y velocity, in mm/sec 00080 00081 Angle yaw; // desired yaw orientation, in deg/100, [0, 36000) 00082 } MAV_DES_POSE; 00083 00084 extern void SDK_mainloop(void); 00085 00086 void sdkInit(void); 00087 00088 inline void writeCommand(short pitch, short roll, short yaw, short thrust, short ctrl, short enable); 00089 inline void sendImuData(void); 00090 inline void sendRcData(void); 00091 inline void sendFlightStateData(void); 00092 inline void sendMavPoseData(void); 00093 inline void sendStatusData(void); 00094 inline void sendCtrlDebugData(void); 00095 00096 inline unsigned short isSerialEnabled(void); 00097 inline void feedbackBeep(void); 00098 inline void estop(void); 00099 00100 inline void processKF(void); 00101 inline void processMotorStateChanges(void); 00102 inline void processFlightActionRequests(void); 00103 inline void processEngageDisengageTimeouts(void); 00104 inline void processLandingThrust(void); 00105 inline void processMotorCommands(void); 00106 inline void processSendData(void); 00107 00109 /*** 00110 * Timestamped packets get send around every 2 s to average the transmission delay. 00111 * Corrects at max 500 us per second. If the time offset is large, the server (host PC) 00112 * time is taken directly and synchronization starts from that time. 00113 */ 00114 inline void synchronizeTime(void); 00115 00117 //inline void watchdog(void); 00118 00120 inline int checkTxPeriod(uint16_t period, uint16_t phase); 00121 00122 struct WO_SDK_STRUCT 00123 { 00124 unsigned char ctrl_mode; 00125 //0x00: Direct individual motor control: individual 00126 //commands for motors 0..3 (AscTec HB, AscTec Pelican) or 0..5 (AscTec FireFly) 00127 //0x01: Direct motor control using standard output 00128 //mapping: commands are interpreted as pitch, roll, yaw and thrust inputs for all motors; 00129 //no attitude controller active ! 00130 //0x02: Attitude and thrust control 00131 //0x03: GPS waypoint navigation. When in GPS mode the vehicle will fly to the desired waypoints 00132 00133 unsigned char ctrl_enabled; 00134 //0x00: Control commands are ignored by LL processor 00135 //0x01: Control commands are accepted by LL processor 00136 00137 //new field 00138 unsigned char disable_motor_onoff_by_stick; // if true, "minimum thrust + full yaw" command will not start/stop motors 00139 }; 00140 extern struct WO_SDK_STRUCT WO_SDK; 00141 00142 //new struct 00143 struct RO_ALL_DATA { 00144 00145 //remote control data 00146 unsigned short channel[8]; 00147 /* 00148 * channel[0]: pitch 00149 * channel[1]: roll 00150 * channel[2]: thrust 00151 * channel[3]: yaw 00152 * channel[4]: serial interface enable/disable (>2048 enabled) 00153 * channel[5]: manual / height control / GPS + height control (0 -> manual mode; 2048 -> height mode; 4095 -> GPS mode) 00154 * 00155 * range of each channel: 0..4095 00156 */ 00157 00158 //angles derived by integration of gyro_outputs, drift compensated by data fusion; -90000..+90000 pitch(nick) and roll, 0..360000 yaw; 1000 = 1 degree 00159 int angle_pitch; 00160 int angle_roll; 00161 int angle_yaw; 00162 00163 //angular velocities, bias free, in 0.0154 °/s (=> 64.8 = 1 °/s) 00164 int angvel_pitch; 00165 int angvel_roll; 00166 int angvel_yaw; 00167 00168 //acc-sensor outputs, calibrated: -10000..+10000 = -1g..+1g, body frame coordinate system 00169 short acc_x; 00170 short acc_y; 00171 short acc_z; 00172 00173 //magnetic field sensors output, offset free and scaled to +-2500 = +- earth field strength; 00174 int Hx; 00175 int Hy; 00176 int Hz; 00177 00178 //RPM measurements (0..200) 00179 /* 00180 * Quadcopter (AscTec Hummingbird, AscTec Pelican) 00181 * 00182 * motor[0]: front 00183 * motor[1]: rear 00184 * motor[2]: left 00185 * motor[3]: right 00186 * 00187 */ 00188 00189 /* 00190 * Hexcopter (AscTec Firefly) 00191 * 00192 * motor[0]: front-left 00193 * motor[1]: left 00194 * motor[2]: rear-left 00195 * motor[3]: rear-right 00196 * motor[4]: right 00197 * motor[5]: front-right 00198 * 00199 */ 00200 unsigned char motor_rpm[6]; 00201 00202 //latitude/longitude in degrees * 10^7 00203 int GPS_latitude; 00204 int GPS_longitude; 00205 00206 //GPS height in mm 00207 int GPS_height; 00208 00209 //speed in x (E/W) and y(N/S) in mm/s 00210 int GPS_speed_x; 00211 int GPS_speed_y; 00212 00213 //GPS heading in deg * 1000 00214 int GPS_heading; 00215 00216 //accuracy estimates in mm and mm/s 00217 unsigned int GPS_position_accuracy; 00218 unsigned int GPS_height_accuracy; 00219 unsigned int GPS_speed_accuracy; 00220 00221 //number of satellites used in NAV solution 00222 unsigned int GPS_sat_num; 00223 00224 // GPS status information: Bit7...Bit3: 0; Bit 2: longitude direction; Bit1: latitude direction; Bit 0: GPS lock 00225 int GPS_status; 00226 00227 unsigned int GPS_time_of_week; //[ms] (1 week = 604,800 s) 00228 unsigned short GPS_week; // starts from beginning of year 1980 00229 00230 //height in mm (after data fusion) 00231 int fusion_height; 00232 00233 //diff. height in mm/s (after data fusion) 00234 int fusion_dheight; 00235 00236 //GPS data fused with all other sensors (best estimations) 00237 int fusion_latitude; //Fused latitude in degrees * 10^7 00238 int fusion_longitude; //Fused longitude in degrees * 10^7 00239 00240 short fusion_speed_x; //[mm/s] 00241 short fusion_speed_y; //[mm/s] 00242 00243 }; //************************************************************ 00244 struct RO_ALL_DATA RO_ALL_Data; 00245 00246 00247 struct RO_RC_DATA 00248 { 00249 unsigned short channel[8]; 00250 /* 00251 * channel[0]: Pitch 00252 * channel[1]: Roll 00253 * channel[2]: Thrust 00254 * channel[3]: Yaw 00255 * channel[4]: Serial interface enable/disable 00256 * channel[5]: manual / height control / GPS + height control 00257 * 00258 * range of each channel: 0..4095 00259 */ 00260 }; 00261 extern struct RO_RC_DATA RO_RC_Data; 00262 00263 00264 //--- send commands ----------------------------------------------------------------------------------------------------------------------------------------------- 00265 //new struct 00266 struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL 00267 { 00268 unsigned char motor[8]; 00269 00270 /* 00271 * commands will be directly interpreted by each motor individually 00272 * 00273 * range: 0..200 = 0..100 %; 0 = motor off! Please check for command != 0 during flight, as a motor restart might take > 1s! 00274 */ 00275 00276 /* 00277 * Quadcopter (AscTec Hummingbird, AscTec Pelican) 00278 * 00279 * motor[0]: front 00280 * motor[1]: rear 00281 * motor[2]: left 00282 * motor[3]: right 00283 * 00284 */ 00285 00286 /* 00287 * Hexcopter (AscTec Firefly) 00288 * 00289 * motor[0]: front-left 00290 * motor[1]: left 00291 * motor[2]: rear-left 00292 * motor[3]: rear-right 00293 * motor[4]: right 00294 * motor[5]: front-right 00295 * 00296 */ 00297 00298 }; 00299 struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL WO_Direct_Individual_Motor_Control; 00300 00301 struct WO_DIRECT_MOTOR_CONTROL 00302 { 00303 unsigned char pitch; 00304 unsigned char roll; 00305 unsigned char yaw; 00306 unsigned char thrust; 00307 00308 /* 00309 * commands will be directly interpreted by the mixer 00310 * running on each of the motor controllers 00311 * 00312 * range (pitch, roll, yaw commands): 0..200 = - 100..+100 % 00313 * range of thrust command: 0..200 = 0..100 % 00314 */ 00315 00316 }; 00317 extern struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control; 00318 00319 struct WO_CTRL_INPUT 00320 { //serial commands (= Scientific Interface) 00321 short pitch; //Pitch input: -2047..+2047 (0=neutral) 00322 short roll; //Roll input: -2047..+2047 (0=neutral) 00323 short yaw; //(=R/C Stick input) -2047..+2047 (0=neutral) 00324 short thrust; //Collective: 0..4095 = 0..100% 00325 short ctrl; /*control byte: 00326 bit 0: pitch control enabled 00327 bit 1: roll control enabled 00328 bit 2: yaw control enabled 00329 bit 3: thrust control enabled 00330 bit 4: Height control enabled 00331 bit 5: GPS position control enabled 00332 */ 00333 }; 00334 extern struct WO_CTRL_INPUT WO_CTRL_Input; 00335 00336 //new part 00337 00338 //waypoint commands 00339 00340 struct WAYPOINT { //waypoint definition 00341 //always set to 1 00342 unsigned int wp_activated; 00343 00344 //use WPPROP_* defines to set waypoint properties 00345 unsigned char properties; 00346 00347 //max. speed to travel to waypoint in % (default 100) 00348 unsigned char max_speed; 00349 00350 //time to stay at a waypoint (XYZ) in 1/100 s 00351 unsigned short time; 00352 00353 //position accuracy to consider a waypoint reached in mm (recommended: 3000 (= 3.0 m)) 00354 unsigned short pos_acc; 00355 00356 //chksum = 0xAAAA + wp.yaw + wp.height + wp.time + wp.X + wp.Y + wp.max_speed + wp.pos_acc + wp.properties + wp.wp_number; 00357 short chksum; 00358 00359 //relative waypoint coordinates in mm // longitude in abs coords e.g. 113647430 (= 11.3647430°; angle in degrees * 10^7) 00360 int X; 00361 //relative waypoint coordinates in mm // latitude in abs coords e.g. 480950480 (= 48.0950480°; angle in degrees * 10^7) 00362 int Y; 00363 00364 //yaw angle 00365 int yaw; // 1/1000° 00366 00367 //height over 0 reference in mm 00368 int height; 00369 00370 }; 00371 extern struct WAYPOINT wpToLL; 00372 00373 //set waypoint properties with WPPROP_* defines (wpToLL.properties) 00374 #define WPPROP_ABSCOORDS 0x01 //if set waypoint is interpreted as absolute coordinates, else relative coordinates 00375 #define WPPROP_HEIGHTENABLED 0x02 //set new height at waypoint 00376 #define WPPROP_YAWENABLED 0x04 //set new yaw-angle at waypoint 00377 #define WPPROP_AUTOMATICGOTO 0x10 //if set, vehicle will not wait for a goto command, but goto this waypoint directly 00378 00379 extern unsigned char wpCtrlWpCmd; //choose actual waypoint command from WP_CMD_* defines 00380 #define WP_CMD_SINGLE_WP 0x01 //fly to single waypoint 00381 #define WP_CMD_LAUNCH 0x02 //launch to 10m at current position 00382 #define WP_CMD_LAND 0x03 //land at current position 00383 #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 00384 #define WP_CMD_SETHOME 0x05 //save current vehicle position as home position 00385 #define WP_CMD_ABORT 0x06 //abort navigation (stops current waypoint flying) 00386 00387 extern unsigned char wpCtrlWpCmdUpdated; //send current waypoint command to LL 00388 extern unsigned char wpCtrlAckTrigger; //acknowledge from LL processor that waypoint was accepted 00389 00390 //Waypoint navigation status 00391 extern unsigned short wpCtrlNavStatus; //check navigation status with WP_NAVSTAT_* defines 00392 #define WP_NAVSTAT_REACHED_POS 0x01 //vehicle has entered a radius of WAYPOINT.pos_acc and time to stay is not necessarily over 00393 #define WP_NAVSTAT_REACHED_POS_TIME 0x02 //vehicle is within a radius of WAYPOINT.pos_acc and time to stay is over 00394 #define WP_NAVSTAT_20M 0x04 //vehicle within a 20m radius of the waypoint 00395 #define WP_NAVSTAT_PILOT_ABORT 0x08 //waypoint navigation aborted by safety pilot (any stick was moved) 00396 00397 extern unsigned short wpCtrlDistToWp; //current distance to the current waypoint in dm (=10 cm) 00398 00399 #endif // SDK_H