Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

UAVXArm.h@1:1e3318a30ddd, 2011-02-25 (annotated)

Committer:
gke
Date:
Fri Feb 25 01:35:24 2011 +0000
Revision:
1:1e3318a30ddd
Parent:
0:62a1c91a859a
Child:
2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1
gke 0:62a1c91a859a 2 // Commissioning defines
gke 0:62a1c91a859a 3
gke 1:1e3318a30ddd 4 #define SW_I2C // define for software I2C - TRAGICALLY SLOW
gke 1:1e3318a30ddd 5
gke 1:1e3318a30ddd 6 #define MAGIC 1.0 // rescales the sensitivity of all PID loop params
gke 0:62a1c91a859a 7
gke 1:1e3318a30ddd 8 #define I2C_MAX_RATE_HZ 400000
gke 0:62a1c91a859a 9
gke 1:1e3318a30ddd 10 #define PWM_UPDATE_HZ 200 // MUST BE LESS THAN OR EQUAL TO 450HZ
gke 0:62a1c91a859a 11
gke 1:1e3318a30ddd 12 #define DISABLE_EXTRAS // suppress altitude hold, position hold and inertial compensation
gke 0:62a1c91a859a 13 #define SUPPRESS_SDCARD // no logging to check if buffering backup is an issue
gke 0:62a1c91a859a 14
gke 0:62a1c91a859a 15 //BMP occasional returns bad results - changes outside this rate are deemed sensor/buss errors
gke 0:62a1c91a859a 16 #define BARO_SANITY_CHECK_MPS 10.0 // dm/S 20,40,60,80 or 100
gke 0:62a1c91a859a 17
gke 1:1e3318a30ddd 18 #define SIX_DOF // effects acc and gyro addresses
gke 0:62a1c91a859a 19
gke 0:62a1c91a859a 20 #define SOFTWARE_CAM_PWM
gke 1:1e3318a30ddd 21
gke 0:62a1c91a859a 22 #define BATTERY_VOLTS_SCALE 57.85 // 51.8144 // Volts scaling for internal divider
gke 0:62a1c91a859a 23
gke 0:62a1c91a859a 24 //#define DCM_YAW_COMP
gke 0:62a1c91a859a 25
gke 0:62a1c91a859a 26 #define USING_MBED
gke 0:62a1c91a859a 27
gke 0:62a1c91a859a 28 // ===============================================================================================
gke 0:62a1c91a859a 29 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 30 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 31 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 0:62a1c91a859a 32 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
gke 0:62a1c91a859a 33 // ===============================================================================================
gke 0:62a1c91a859a 34
gke 0:62a1c91a859a 35 // This is part of UAVXArm.
gke 0:62a1c91a859a 36
gke 0:62a1c91a859a 37 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 38 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 39 // License, or (at your option) any later version.
gke 0:62a1c91a859a 40
gke 0:62a1c91a859a 41 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 42 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 43 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 44
gke 0:62a1c91a859a 45 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 46 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 47
gke 0:62a1c91a859a 48 #include "mbed.h"
gke 0:62a1c91a859a 49 #include "SDFileSystem.h"
gke 0:62a1c91a859a 50 #include "SerialBuffered.h" // used in preference to MODSERIAL
gke 0:62a1c91a859a 51
gke 0:62a1c91a859a 52 // Additional Types
gke 0:62a1c91a859a 53 typedef uint8_t uint8 ;
gke 0:62a1c91a859a 54 typedef int8_t int8;
gke 0:62a1c91a859a 55 typedef uint16_t uint16;
gke 0:62a1c91a859a 56 typedef int16_t int16;
gke 0:62a1c91a859a 57 typedef int32_t int24;
gke 0:62a1c91a859a 58 typedef uint32_t uint24;
gke 0:62a1c91a859a 59 typedef int32_t int32;
gke 0:62a1c91a859a 60 typedef uint32_t uint32;
gke 0:62a1c91a859a 61 typedef uint8_t boolean;
gke 0:62a1c91a859a 62 typedef float real32;
gke 0:62a1c91a859a 63
gke 0:62a1c91a859a 64 extern Timer timer;
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 //________________________________________________________________________________________
gke 0:62a1c91a859a 67
gke 0:62a1c91a859a 68
gke 0:62a1c91a859a 69 // Useful Constants
gke 0:62a1c91a859a 70 #define NUL 0
gke 0:62a1c91a859a 71 #define SOH 1
gke 0:62a1c91a859a 72 #define EOT 4
gke 0:62a1c91a859a 73 #define ACK 6
gke 0:62a1c91a859a 74 #define HT 9
gke 0:62a1c91a859a 75 #define LF 10
gke 0:62a1c91a859a 76 #define CR 13
gke 0:62a1c91a859a 77 #define NAK 21
gke 0:62a1c91a859a 78 #define ESC 27
gke 0:62a1c91a859a 79 #define true 1
gke 0:62a1c91a859a 80 #define false 0
gke 0:62a1c91a859a 81
gke 0:62a1c91a859a 82 #define PI 3.141592654
gke 0:62a1c91a859a 83 #define HALFPI (PI*0.5)
gke 0:62a1c91a859a 84 #define QUARTERPI (PI*0.25)
gke 0:62a1c91a859a 85 #define SIXTHPI (PI/6.0)
gke 0:62a1c91a859a 86 #define TWOPI (PI*2.0)
gke 0:62a1c91a859a 87 #define RADDEG (180.0/PI)
gke 0:62a1c91a859a 88 #define MILLIANGLE (180000.0/PI)
gke 0:62a1c91a859a 89 #define DEGRAD (PI/180.0)
gke 0:62a1c91a859a 90
gke 0:62a1c91a859a 91 #define MILLIRAD 18
gke 0:62a1c91a859a 92 #define CENTIRAD 2
gke 0:62a1c91a859a 93
gke 0:62a1c91a859a 94 #define MAXINT32 0x7fffffff
gke 0:62a1c91a859a 95 #define MAXINT16 0x7fff
gke 0:62a1c91a859a 96
gke 0:62a1c91a859a 97 //#define BATCHMODE
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 #ifndef BATCHMODE
gke 0:62a1c91a859a 100 //#define RX6CH
gke 0:62a1c91a859a 101 //#define EXPERIMENTAL
gke 0:62a1c91a859a 102 //#define TESTING
gke 0:62a1c91a859a 103 //#define RX6CH // 6ch Receivers
gke 0:62a1c91a859a 104 //#define SIMULATE
gke 0:62a1c91a859a 105 //#define VTCOPTER
gke 0:62a1c91a859a 106 //#define Y6COPTER
gke 0:62a1c91a859a 107 #define QUADROCOPTER
gke 0:62a1c91a859a 108 //#define TRICOPTER
gke 0:62a1c91a859a 109 //#define HELICOPTER
gke 0:62a1c91a859a 110 //#define AILERON
gke 0:62a1c91a859a 111 // #define ELEVON
gke 0:62a1c91a859a 112 #endif // !BATCHMODE
gke 0:62a1c91a859a 113
gke 0:62a1c91a859a 114 //________________________________________________________________________________________________
gke 0:62a1c91a859a 115
gke 0:62a1c91a859a 116 #define USE_PPM_FAILSAFE
gke 0:62a1c91a859a 117
gke 0:62a1c91a859a 118 // Airframe
gke 0:62a1c91a859a 119
gke 0:62a1c91a859a 120 #if ( defined TRICOPTER | defined QUADROCOPTER | defined VTCOPTER | defined Y6COPTER )
gke 0:62a1c91a859a 121 #define MULTICOPTER
gke 0:62a1c91a859a 122 #endif
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 #if ( defined HELICOPTER | defined AILERON | defined ELEVON )
gke 0:62a1c91a859a 125 #if ( defined AILERON | defined ELEVON )
gke 0:62a1c91a859a 126 #define NAV_WING
gke 0:62a1c91a859a 127 #endif
gke 0:62a1c91a859a 128 #endif
gke 0:62a1c91a859a 129
gke 0:62a1c91a859a 130 #ifdef QUADROCOPTER
gke 0:62a1c91a859a 131 #define AF_TYPE QuadAF
gke 0:62a1c91a859a 132 #endif
gke 0:62a1c91a859a 133 #ifdef TRICOPTER
gke 0:62a1c91a859a 134 #define AF_TYPE TriAF
gke 0:62a1c91a859a 135 #endif
gke 0:62a1c91a859a 136 #ifdef VTCOPTER
gke 0:62a1c91a859a 137 #define AF_TYPE VTAF
gke 0:62a1c91a859a 138 #endif
gke 0:62a1c91a859a 139 #ifdef Y6COPTER
gke 0:62a1c91a859a 140 #define AF_TYPE Y6AF
gke 0:62a1c91a859a 141 #endif
gke 0:62a1c91a859a 142 #ifdef HELICOPTER
gke 0:62a1c91a859a 143 #define AF_TYPE HeliAF
gke 0:62a1c91a859a 144 #endif
gke 0:62a1c91a859a 145 #ifdef ELEVON
gke 0:62a1c91a859a 146 #define AF_TYPE ElevAF
gke 0:62a1c91a859a 147 #endif
gke 0:62a1c91a859a 148 #ifdef AILERON
gke 0:62a1c91a859a 149 #define AF_TYPE AilAF
gke 0:62a1c91a859a 150 #endif
gke 0:62a1c91a859a 151
gke 0:62a1c91a859a 152 #define GPS_INC_GROUNDSPEED // GPS groundspeed is not used for flight but may be of interest
gke 0:62a1c91a859a 153
gke 0:62a1c91a859a 154 // Timeouts and Update Intervals
gke 0:62a1c91a859a 155
gke 0:62a1c91a859a 156 #define FAILSAFE_TIMEOUT_MS 1000L // mS. hold last "good" settings and then restore flight or abort
gke 0:62a1c91a859a 157 #define ABORT_TIMEOUT_GPS_MS 5000L // mS. go to descend on position hold if GPS valid.
gke 0:62a1c91a859a 158 #define ABORT_TIMEOUT_NO_GPS_MS 0L // mS. go to descend on position hold if GPS valid.
gke 0:62a1c91a859a 159 #define ABORT_UPDATE_MS 1000L // mS. retry period for RC Signal and restore Pilot in Control
gke 0:62a1c91a859a 160 #define ARMED_TIMEOUT_MS 150000L // mS. automatic disarming if armed for this long and landed
gke 0:62a1c91a859a 161
gke 0:62a1c91a859a 162 #define ALT_DESCENT_UPDATE_MS 1000L // mS time between throttle reduction clicks in failsafe descent without baro
gke 0:62a1c91a859a 163
gke 0:62a1c91a859a 164 #define RC_STICK_MOVEMENT 5L // minimum to be recognised as a stick input change without triggering failsafe
gke 0:62a1c91a859a 165
gke 0:62a1c91a859a 166 #define THROTTLE_LOW_DELAY_MS 1000L // mS. that motor runs at idle after the throttle is closed
gke 0:62a1c91a859a 167 #define THROTTLE_UPDATE_MS 3000L // mS. constant throttle time for altitude hold
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 #define NAV_ACTIVE_DELAY_MS 10000L // mS. after throttle exceeds idle that Nav becomes active
gke 0:62a1c91a859a 170 #define NAV_RTH_LAND_TIMEOUT_MS 10000L // mS. Shutdown throttle if descent lasts too long
gke 0:62a1c91a859a 171
gke 0:62a1c91a859a 172 #define UAVX_TEL_INTERVAL_MS 125L // mS. emit an interleaved telemetry packet
gke 0:62a1c91a859a 173 #define ARDU_TEL_INTERVAL_MS 200L // mS. Ardustation
gke 0:62a1c91a859a 174 #define UAVX_CONTROL_TEL_INTERVAL_MS 10L // mS. flight control only
gke 0:62a1c91a859a 175 #define CUSTOM_TEL_INTERVAL_MS 250L // mS.
gke 0:62a1c91a859a 176 #define UAVX_MIN_TEL_INTERVAL_MS 1000L // mS. emit minimum FPV telemetry packet slow rate for example to FrSky
gke 0:62a1c91a859a 177
gke 0:62a1c91a859a 178 #define GPS_TIMEOUT_MS 2000L // mS.
gke 0:62a1c91a859a 179
gke 0:62a1c91a859a 180 #define ALT_UPDATE_HZ 20L // Hz based on 50mS update time for Baro
gke 0:62a1c91a859a 181
gke 0:62a1c91a859a 182 #ifdef MULTICOPTER
gke 0:62a1c91a859a 183 //#define PWM_UPDATE_HZ 450L // PWM motor update rate must be <450 and >100
gke 0:62a1c91a859a 184 #else
gke 0:62a1c91a859a 185 #define PWM_UPDATE_HZ 40L // standard RC servo update rate
gke 0:62a1c91a859a 186 #endif // MULTICOPTER
gke 0:62a1c91a859a 187
gke 0:62a1c91a859a 188 // Accelerometers
gke 0:62a1c91a859a 189
gke 0:62a1c91a859a 190 #define DAMP_HORIZ_LIMIT 3L // equivalent stick units - no larger than 5
gke 0:62a1c91a859a 191 #define DAMP_VERT_LIMIT_LOW -5L // maximum throttle reduction
gke 0:62a1c91a859a 192 #define DAMP_VERT_LIMIT_HIGH 20L // maximum throttle increase
gke 0:62a1c91a859a 193
gke 0:62a1c91a859a 194 // Gyros
gke 0:62a1c91a859a 195
gke 0:62a1c91a859a 196 #define ATTITUDE_FF_DIFF (24.0/16.0) // 0 - 32 max feedforward speeds up roll/pitch recovery on fast stick change
gke 0:62a1c91a859a 197
gke 0:62a1c91a859a 198 #define ATTITUDE_ENABLE_DECAY // enables decay to zero angle when roll/pitch is not in fact zero!
gke 0:62a1c91a859a 199 // unfortunately there seems to be a leak which cause the roll/pitch
gke 0:62a1c91a859a 200 // to increase without the decay.
gke 0:62a1c91a859a 201
gke 0:62a1c91a859a 202 #define ATTITUDE_SCALE 0.5 // scaling of stick units for attitude angle control
gke 0:62a1c91a859a 203
gke 0:62a1c91a859a 204 // Enable "Dynamic mass" compensation Roll and/or Pitch
gke 0:62a1c91a859a 205 // Normally disabled for pitch only
gke 0:62a1c91a859a 206 //#define DISABLE_DYNAMIC_MASS_COMP_ROLL
gke 0:62a1c91a859a 207 //#define DISABLE_DYNAMIC_MASS_COMP_PITCH
gke 0:62a1c91a859a 208
gke 0:62a1c91a859a 209 // Altitude Hold
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211
gke 0:62a1c91a859a 212 #define ALT_HOLD_MAX_ROC_MPS 0.5 // Must be changing altitude at less than this for alt. hold to be detected
gke 0:62a1c91a859a 213
gke 0:62a1c91a859a 214 // the range within which throttle adjustment is proportional to altitude error
gke 0:62a1c91a859a 215 #define ALT_BAND_M 5.0 // Metres
gke 0:62a1c91a859a 216
gke 0:62a1c91a859a 217 #define LAND_M 3.0 // Metres deemed to have landed when below this height
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 #define ALT_MAX_THR_COMP 40L // Stick units was 32
gke 0:62a1c91a859a 220
gke 0:62a1c91a859a 221 #define ALT_INT_WINDUP_LIMIT 16L
gke 0:62a1c91a859a 222
gke 0:62a1c91a859a 223 #define ALT_RF_ENABLE_M 5.0 // altitude below which the rangefiner is selected as the altitude source
gke 0:62a1c91a859a 224 #define ALT_RF_DISABLE_M 6.0 // altitude above which the rangefiner is deselected as the altitude source
gke 0:62a1c91a859a 225
gke 0:62a1c91a859a 226 // Navigation
gke 0:62a1c91a859a 227
gke 0:62a1c91a859a 228 #define NAV_ACQUIRE_BEEPER
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 //#define ATTITUDE_NO_LIMITS // full stick range is available otherwise it is scaled to Nav sensitivity
gke 0:62a1c91a859a 231
gke 0:62a1c91a859a 232 #define NAV_RTH_LOCKOUT ((10.0*PI)/180.0) // first number is degrees
gke 0:62a1c91a859a 233
gke 0:62a1c91a859a 234 #define NAV_MAX_ROLL_PITCH 25L // Rx stick units
gke 0:62a1c91a859a 235 #define NAV_CONTROL_HEADROOM 10L // at least this much stick control headroom above Nav control
gke 0:62a1c91a859a 236 #define NAV_DIFF_LIMIT 24L // Approx double NAV_INT_LIMIT
gke 0:62a1c91a859a 237 #define NAV_INT_WINDUP_LIMIT 64L // ???
gke 0:62a1c91a859a 238
gke 0:62a1c91a859a 239 #define NAV_ENFORCE_ALTITUDE_CEILING // limit all autonomous altitudes
gke 0:62a1c91a859a 240 #define NAV_CEILING 120L // 400 feet
gke 0:62a1c91a859a 241 #define NAV_MAX_NEUTRAL_RADIUS 3L // Metres also minimum closing radius
gke 0:62a1c91a859a 242 #define NAV_MAX_RADIUS 99L // Metres
gke 0:62a1c91a859a 243
gke 0:62a1c91a859a 244 #ifdef NAV_WING
gke 0:62a1c91a859a 245 #define NAV_PROXIMITY_RADIUS 20.0 // Metres if there are no WPs
gke 0:62a1c91a859a 246 #define NAV_PROXIMITY_ALTITUDE 5.0 // Metres
gke 0:62a1c91a859a 247 #else
gke 0:62a1c91a859a 248 #define NAV_PROXIMITY_RADIUS 5.0 // Metres if there are no WPs
gke 0:62a1c91a859a 249 #define NAV_PROXIMITY_ALTITUDE 3.0 // Metres
gke 0:62a1c91a859a 250 #endif // NAV_WING
gke 0:62a1c91a859a 251
gke 0:62a1c91a859a 252 // reads $GPGGA sentence - all others discarded
gke 0:62a1c91a859a 253
gke 0:62a1c91a859a 254 #define GPS_MIN_SATELLITES 6 // preferably > 5 for 3D fix
gke 0:62a1c91a859a 255 #define GPS_MIN_FIX 1 // must be 1 or 2
gke 0:62a1c91a859a 256 #define GPS_ORIGIN_SENTENCES 30L // Number of sentences needed to obtain reasonable Origin
gke 0:62a1c91a859a 257 #define GPS_MIN_HDILUTE 130L // HDilute * 100
gke 0:62a1c91a859a 258
gke 0:62a1c91a859a 259 #define NAV_SENS_THRESHOLD 40L // Navigation disabled if Ch7 is less than this
gke 0:62a1c91a859a 260 #define NAV_SENS_ALTHOLD_THRESHOLD 20L // Altitude hold disabled if Ch7 is less than this
gke 0:62a1c91a859a 261 #define NAV_SENS_6CH 80L // Low GPS gain for 6ch Rx
gke 0:62a1c91a859a 262
gke 0:62a1c91a859a 263 #define NAV_YAW_LIMIT 10L // yaw slew rate for RTH
gke 0:62a1c91a859a 264 #define NAV_MAX_TRIM 20L // max trim offset for altitude hold
gke 0:62a1c91a859a 265 #define NAV_CORR_SLEW_LIMIT 1L // *5L maximum change in roll or pitch correction per GPS update
gke 0:62a1c91a859a 266
gke 0:62a1c91a859a 267 #define ATTITUDE_HOLD_LIMIT 8L // dead zone for roll/pitch stick for position hold
gke 0:62a1c91a859a 268 #define ATTITUDE_HOLD_RESET_INTERVAL 25L // number of impulse cycles before GPS position is re-acquired
gke 0:62a1c91a859a 269
gke 0:62a1c91a859a 270 //#define NAV_PPM_FAILSAFE_RTH // PPM signal failure causes RTH with Signal sampled periodically
gke 0:62a1c91a859a 271
gke 0:62a1c91a859a 272 // Throttle
gke 0:62a1c91a859a 273
gke 0:62a1c91a859a 274 #define THROTTLE_MAX_CURRENT 40L // Amps total current at full throttle for estimated mAH
gke 0:62a1c91a859a 275 #define CURRENT_SENSOR_MAX 50L // Amps range of current sensor - used for estimated consumption - no actual sensor yet.
gke 0:62a1c91a859a 276 #define THROTTLE_CURRENT_SCALE ((THROTTLE_MAX_CURRENT * 1024L)/(200L * CURRENT_SENSOR_MAX ))
gke 0:62a1c91a859a 277
gke 0:62a1c91a859a 278 #define THROTTLE_SLEW_LIMIT 0 // limits the rate at which the throttle can change (=0 no slew limit, 5 OK)
gke 0:62a1c91a859a 279 #define THROTTLE_MIDDLE 10 // throttle stick dead zone for baro
gke 0:62a1c91a859a 280 #define THROTTLE_MIN_ALT_HOLD 75 // min throttle stick for altitude lock
gke 0:62a1c91a859a 281
gke 0:62a1c91a859a 282 // RC
gke 0:62a1c91a859a 283
gke 0:62a1c91a859a 284 #define RC_INIT_FRAMES 32 // number of initial RC frames to allow filters to settle
gke 0:62a1c91a859a 285
gke 0:62a1c91a859a 286 #define RC_MIN_WIDTH_US 900
gke 0:62a1c91a859a 287 #define RC_MAX_WIDTH_US 2100
gke 0:62a1c91a859a 288
gke 0:62a1c91a859a 289 #define RC_NO_CHANGE_TIMEOUT_MS 20000L // mS.
gke 0:62a1c91a859a 290
gke 0:62a1c91a859a 291 typedef union {
gke 0:62a1c91a859a 292 int16 i16;
gke 0:62a1c91a859a 293 uint16 u16;
gke 0:62a1c91a859a 294 struct {
gke 0:62a1c91a859a 295 uint8 b0;
gke 0:62a1c91a859a 296 uint8 b1;
gke 0:62a1c91a859a 297 };
gke 0:62a1c91a859a 298 struct {
gke 0:62a1c91a859a 299 int8 pad;
gke 0:62a1c91a859a 300 int8 i1;
gke 0:62a1c91a859a 301 };
gke 0:62a1c91a859a 302 } i16u;
gke 0:62a1c91a859a 303
gke 0:62a1c91a859a 304 typedef union {
gke 0:62a1c91a859a 305 int24 i24;
gke 0:62a1c91a859a 306 uint24 u24;
gke 0:62a1c91a859a 307 struct {
gke 0:62a1c91a859a 308 uint8 b0;
gke 0:62a1c91a859a 309 uint8 b1;
gke 0:62a1c91a859a 310 uint8 b2;
gke 0:62a1c91a859a 311 };
gke 0:62a1c91a859a 312 struct {
gke 0:62a1c91a859a 313 uint8 pad;
gke 0:62a1c91a859a 314 int16 i2_1;
gke 0:62a1c91a859a 315 };
gke 0:62a1c91a859a 316 } i24u;
gke 0:62a1c91a859a 317
gke 0:62a1c91a859a 318 typedef union {
gke 0:62a1c91a859a 319 int32 i32;
gke 0:62a1c91a859a 320 uint32 u32;
gke 0:62a1c91a859a 321 struct {
gke 0:62a1c91a859a 322 uint8 b0;
gke 0:62a1c91a859a 323 uint8 b1;
gke 0:62a1c91a859a 324 uint8 b2;
gke 0:62a1c91a859a 325 uint8 b3;
gke 0:62a1c91a859a 326 };
gke 0:62a1c91a859a 327 struct {
gke 0:62a1c91a859a 328 uint16 w0;
gke 0:62a1c91a859a 329 uint16 w1;
gke 0:62a1c91a859a 330 };
gke 0:62a1c91a859a 331 struct {
gke 0:62a1c91a859a 332 int16 pad;
gke 0:62a1c91a859a 333 int16 iw1;
gke 0:62a1c91a859a 334 };
gke 0:62a1c91a859a 335
gke 0:62a1c91a859a 336 struct {
gke 0:62a1c91a859a 337 uint8 padding;
gke 0:62a1c91a859a 338 int24 i3_1;
gke 0:62a1c91a859a 339 };
gke 0:62a1c91a859a 340 } i32u;
gke 0:62a1c91a859a 341
gke 0:62a1c91a859a 342 #define TX_BUFF_MASK 511
gke 0:62a1c91a859a 343 #define RX_BUFF_MASK 511
gke 0:62a1c91a859a 344
gke 0:62a1c91a859a 345 typedef struct { // PPM
gke 0:62a1c91a859a 346 uint8 Head;
gke 0:62a1c91a859a 347 int16 B[4][8];
gke 0:62a1c91a859a 348 } int16x8x4Q;
gke 0:62a1c91a859a 349
gke 0:62a1c91a859a 350 typedef struct { // Baro
gke 0:62a1c91a859a 351 uint8 Head, Tail;
gke 0:62a1c91a859a 352 int24 B[8];
gke 0:62a1c91a859a 353 } int24x8Q;
gke 0:62a1c91a859a 354
gke 0:62a1c91a859a 355 typedef struct { // GPS
gke 0:62a1c91a859a 356 uint8 Head, Tail;
gke 0:62a1c91a859a 357 int32 Lat[4], Lon[4];
gke 0:62a1c91a859a 358 } int32x4Q;
gke 0:62a1c91a859a 359
gke 0:62a1c91a859a 360 // Macros
gke 0:62a1c91a859a 361
gke 0:62a1c91a859a 362 #define Sign(i) (((i)<0) ? -1 : 1)
gke 0:62a1c91a859a 363
gke 0:62a1c91a859a 364 #define Max(i,j) ((i<j) ? j : i)
gke 0:62a1c91a859a 365 #define Min(i,j) ((i<j) ? i : j )
gke 0:62a1c91a859a 366 #define Decay1(i) (((i) < 0) ? (i+1) : (((i) > 0) ? (i-1) : 0))
gke 0:62a1c91a859a 367 #define Limit(i,l,u) (((i) < l) ? l : (((i) > u) ? u : (i)))
gke 0:62a1c91a859a 368 #define Sqr(v) ( v * v )
gke 0:62a1c91a859a 369
gke 0:62a1c91a859a 370 // To speed up NMEA sentence processing
gke 0:62a1c91a859a 371 // must have a positive argument
gke 0:62a1c91a859a 372 #define ConvertDDegToMPi(d) (((int32)d * 3574L)>>11)
gke 0:62a1c91a859a 373 #define ConvertMPiToDDeg(d) (((int32)d * 2048L)/3574L)
gke 0:62a1c91a859a 374
gke 0:62a1c91a859a 375 #define ToPercent(n, m) (((n)*100L)/m)
gke 0:62a1c91a859a 376
gke 0:62a1c91a859a 377 // Simple filters using weighted averaging
gke 0:62a1c91a859a 378 #define VerySoftFilter(O,N) (SRS16((O)+(N)*3, 2))
gke 0:62a1c91a859a 379 #define SoftFilter(O,N) (SRS16((O)+(N), 1))
gke 0:62a1c91a859a 380 #define MediumFilter(O,N) (SRS16((O)*3+(N), 2))
gke 0:62a1c91a859a 381 #define HardFilter(O,N) (SRS16((O)*7+(N), 3))
gke 0:62a1c91a859a 382
gke 0:62a1c91a859a 383 // Unsigned
gke 0:62a1c91a859a 384 #define VerySoftFilterU(O,N) (((O)+(N)*3+2)>>2)
gke 0:62a1c91a859a 385 #define SoftFilterU(O,N) (((O)+(N)+1)>>1)
gke 0:62a1c91a859a 386 #define MediumFilterU(O,N) (((O)*3+(N)+2)>>2)
gke 0:62a1c91a859a 387 #define HardFilterU(O,N) (((O)*7+(N)+4)>>3)
gke 0:62a1c91a859a 388
gke 0:62a1c91a859a 389 #define NoFilter(O,N) (N)
gke 0:62a1c91a859a 390
gke 0:62a1c91a859a 391 #define DisableInterrupts (INTCONbits.GIEH=0)
gke 0:62a1c91a859a 392 #define EnableInterrupts (INTCONbits.GIEH=1)
gke 0:62a1c91a859a 393 #define InterruptsEnabled (INTCONbits.GIEH)
gke 0:62a1c91a859a 394
gke 0:62a1c91a859a 395 // PARAMS
gke 0:62a1c91a859a 396
gke 0:62a1c91a859a 397 #define PARAMS_ADDR_PX 0 // code assumes zero!
gke 0:62a1c91a859a 398 #define MAX_PARAMETERS 64 // parameters in PXPROM start at zero
gke 0:62a1c91a859a 399
gke 0:62a1c91a859a 400 #define STATS_ADDR_PX ( PARAMS_ADDR_PX + (MAX_PARAMETERS *2) )
gke 0:62a1c91a859a 401 #define MAX_STATS 64
gke 0:62a1c91a859a 402
gke 0:62a1c91a859a 403 // uses second Page of PXPROM
gke 0:62a1c91a859a 404 #define NAV_ADDR_PX 256L
gke 0:62a1c91a859a 405 // 0 - 8 not used
gke 0:62a1c91a859a 406
gke 0:62a1c91a859a 407 #define NAV_NO_WP (NAV_ADDR_PX + 9)
gke 0:62a1c91a859a 408 #define NAV_PROX_ALT (NAV_ADDR_PX + 10)
gke 0:62a1c91a859a 409 #define NAV_PROX_RADIUS (NAV_ADDR_PX + 11)
gke 0:62a1c91a859a 410 #define NAV_ORIGIN_ALT (NAV_ADDR_PX + 12)
gke 0:62a1c91a859a 411 #define NAV_ORIGIN_LAT (NAV_ADDR_PX + 14)
gke 0:62a1c91a859a 412 #define NAV_ORIGIN_LON (NAV_ADDR_PX + 18)
gke 0:62a1c91a859a 413 #define NAV_RTH_ALT_HOLD (NAV_ADDR_PX + 22) // P[NavRTHAlt]
gke 0:62a1c91a859a 414 #define NAV_WP_START (NAV_ADDR_PX + 24)
gke 0:62a1c91a859a 415
gke 0:62a1c91a859a 416 #define WAYPOINT_REC_SIZE (4 + 4 + 2 + 1) // Lat + Lon + Alt + Loiter
gke 0:62a1c91a859a 417 #define NAV_MAX_WAYPOINTS ((256 - 24 - 1)/WAYPOINT_REC_SIZE)
gke 0:62a1c91a859a 418
gke 0:62a1c91a859a 419 //______________________________________________________________________________________________
gke 0:62a1c91a859a 420
gke 0:62a1c91a859a 421 // main.c
gke 0:62a1c91a859a 422
gke 0:62a1c91a859a 423 enum Directions {BF, LR, UD, Alt }; // x forward, y left and z down
gke 0:62a1c91a859a 424 enum Angles { Pitch, Roll, Yaw }; // X, Y, Z
gke 0:62a1c91a859a 425
gke 0:62a1c91a859a 426 #define FLAG_BYTES 10
gke 0:62a1c91a859a 427 #define TELEMETRY_FLAG_BYTES 6
gke 0:62a1c91a859a 428 typedef union {
gke 0:62a1c91a859a 429 uint8 AllFlags[FLAG_BYTES];
gke 0:62a1c91a859a 430 struct { // Order of these flags subject to change
gke 0:62a1c91a859a 431 uint8
gke 0:62a1c91a859a 432 AltHoldEnabled:
gke 0:62a1c91a859a 433 1,
gke 0:62a1c91a859a 434 AllowTurnToWP:
gke 0:62a1c91a859a 435 1, // stick programmed
gke 0:62a1c91a859a 436 GyroFailure:
gke 0:62a1c91a859a 437 1,
gke 0:62a1c91a859a 438 LostModel:
gke 0:62a1c91a859a 439 1,
gke 0:62a1c91a859a 440 NearLevel:
gke 0:62a1c91a859a 441 1,
gke 0:62a1c91a859a 442 LowBatt:
gke 0:62a1c91a859a 443 1,
gke 0:62a1c91a859a 444 GPSValid:
gke 0:62a1c91a859a 445 1,
gke 0:62a1c91a859a 446 NavValid:
gke 0:62a1c91a859a 447 1,
gke 0:62a1c91a859a 448
gke 0:62a1c91a859a 449 BaroFailure:
gke 0:62a1c91a859a 450 1,
gke 0:62a1c91a859a 451 AccFailure:
gke 0:62a1c91a859a 452 1,
gke 0:62a1c91a859a 453 CompassFailure:
gke 0:62a1c91a859a 454 1,
gke 0:62a1c91a859a 455 GPSFailure:
gke 0:62a1c91a859a 456 1,
gke 0:62a1c91a859a 457 AttitudeHold:
gke 0:62a1c91a859a 458 1,
gke 0:62a1c91a859a 459 ThrottleMoving:
gke 0:62a1c91a859a 460 1,
gke 0:62a1c91a859a 461 HoldingAlt:
gke 0:62a1c91a859a 462 1,
gke 0:62a1c91a859a 463 Navigate:
gke 0:62a1c91a859a 464 1,
gke 0:62a1c91a859a 465
gke 0:62a1c91a859a 466 ReturnHome:
gke 0:62a1c91a859a 467 1,
gke 0:62a1c91a859a 468 WayPointAchieved:
gke 0:62a1c91a859a 469 1,
gke 0:62a1c91a859a 470 WayPointCentred:
gke 0:62a1c91a859a 471 1,
gke 0:62a1c91a859a 472 Unused2: // was UsingGPSAlt:
gke 0:62a1c91a859a 473 1,
gke 0:62a1c91a859a 474 UsingRTHAutoDescend:
gke 0:62a1c91a859a 475 1,
gke 0:62a1c91a859a 476 BaroAltitudeValid:
gke 0:62a1c91a859a 477 1,
gke 0:62a1c91a859a 478 RangefinderAltitudeValid:
gke 0:62a1c91a859a 479 1,
gke 0:62a1c91a859a 480 UsingRangefinderAlt:
gke 0:62a1c91a859a 481 1,
gke 0:62a1c91a859a 482
gke 0:62a1c91a859a 483 // internal flags not really useful externally
gke 0:62a1c91a859a 484
gke 0:62a1c91a859a 485 AllowNavAltitudeHold:
gke 0:62a1c91a859a 486 1, // stick programmed
gke 0:62a1c91a859a 487 UsingPositionHoldLock:
gke 0:62a1c91a859a 488 1,
gke 0:62a1c91a859a 489 Ch5Active:
gke 0:62a1c91a859a 490 1,
gke 0:62a1c91a859a 491 Simulation:
gke 0:62a1c91a859a 492 1,
gke 0:62a1c91a859a 493 AcquireNewPosition:
gke 0:62a1c91a859a 494 1,
gke 0:62a1c91a859a 495 MotorsArmed:
gke 0:62a1c91a859a 496 1,
gke 0:62a1c91a859a 497 NavigationActive:
gke 0:62a1c91a859a 498 1,
gke 0:62a1c91a859a 499 ForceFailsafe:
gke 0:62a1c91a859a 500 1,
gke 0:62a1c91a859a 501
gke 0:62a1c91a859a 502 Signal:
gke 0:62a1c91a859a 503 1,
gke 0:62a1c91a859a 504 RCFrameOK:
gke 0:62a1c91a859a 505 1,
gke 0:62a1c91a859a 506 ParametersValid:
gke 0:62a1c91a859a 507 1,
gke 0:62a1c91a859a 508 RCNewValues:
gke 0:62a1c91a859a 509 1,
gke 0:62a1c91a859a 510 NewCommands:
gke 0:62a1c91a859a 511 1,
gke 0:62a1c91a859a 512 AccelerationsValid:
gke 0:62a1c91a859a 513 1,
gke 0:62a1c91a859a 514 CompassValid:
gke 0:62a1c91a859a 515 1,
gke 0:62a1c91a859a 516 CompassMissRead:
gke 0:62a1c91a859a 517 1,
gke 0:62a1c91a859a 518
gke 0:62a1c91a859a 519 UsingPolarCoordinates:
gke 0:62a1c91a859a 520 1,
gke 0:62a1c91a859a 521 UsingAngleControl:
gke 0:62a1c91a859a 522 1,
gke 0:62a1c91a859a 523 GPSPacketReceived:
gke 0:62a1c91a859a 524 1,
gke 0:62a1c91a859a 525 NavComputed:
gke 0:62a1c91a859a 526 1,
gke 0:62a1c91a859a 527 AltitudeValid:
gke 0:62a1c91a859a 528 1,
gke 0:62a1c91a859a 529 UsingSerialPPM:
gke 0:62a1c91a859a 530 1,
gke 0:62a1c91a859a 531 UsingTxMode2:
gke 0:62a1c91a859a 532 1,
gke 0:62a1c91a859a 533 UsingAltOrientation:
gke 0:62a1c91a859a 534 1,
gke 0:62a1c91a859a 535
gke 0:62a1c91a859a 536 // outside telemetry flags
gke 0:62a1c91a859a 537
gke 0:62a1c91a859a 538 UsingTelemetry:
gke 0:62a1c91a859a 539 1,
gke 0:62a1c91a859a 540 TxToBuffer:
gke 0:62a1c91a859a 541 1,
gke 0:62a1c91a859a 542 NewBaroValue:
gke 0:62a1c91a859a 543 1,
gke 0:62a1c91a859a 544 BeeperInUse:
gke 0:62a1c91a859a 545 1,
gke 0:62a1c91a859a 546 RFInInches:
gke 0:62a1c91a859a 547 1,
gke 0:62a1c91a859a 548 FirstArmed:
gke 0:62a1c91a859a 549
gke 0:62a1c91a859a 550 1,
gke 0:62a1c91a859a 551 HaveGPRMC:
gke 0:62a1c91a859a 552 1,
gke 0:62a1c91a859a 553 UsingPolar:
gke 0:62a1c91a859a 554 1,
gke 0:62a1c91a859a 555 RTCValid:
gke 0:62a1c91a859a 556 1,
gke 0:62a1c91a859a 557 SDCardValid:
gke 0:62a1c91a859a 558 1,
gke 0:62a1c91a859a 559 PXImageStale:
gke 0:62a1c91a859a 560 1,
gke 0:62a1c91a859a 561 UsingLEDDriver:
gke 0:62a1c91a859a 562 1,
gke 0:62a1c91a859a 563 Using9DOF:
gke 0:62a1c91a859a 564 1,
gke 0:62a1c91a859a 565 HaveBatterySensor:
gke 0:62a1c91a859a 566 1;
gke 0:62a1c91a859a 567 };
gke 0:62a1c91a859a 568 } Flags;
gke 0:62a1c91a859a 569
gke 0:62a1c91a859a 570 enum FlightStates { Starting, Landing, Landed, Shutdown, InFlight};
gke 0:62a1c91a859a 571 extern volatile Flags F;
gke 0:62a1c91a859a 572 extern int8 State;
gke 0:62a1c91a859a 573
gke 0:62a1c91a859a 574 //______________________________________________________________________________________________
gke 0:62a1c91a859a 575
gke 0:62a1c91a859a 576 // accel.c
gke 0:62a1c91a859a 577
gke 0:62a1c91a859a 578 #define ACC_FREQ 50 // Hz must be less than 100Hz
gke 0:62a1c91a859a 579 const real32 OneOnTwoPiAccF = ( 1.0 / ( TWOPI * ACC_FREQ ));
gke 0:62a1c91a859a 580
gke 0:62a1c91a859a 581 enum AccelerometerTypes { LISLAcc, ADXL345Acc, AccUnknown };
gke 0:62a1c91a859a 582
gke 0:62a1c91a859a 583 extern void ShowAccType(void);
gke 0:62a1c91a859a 584 extern void ReadAccelerometers(void);
gke 0:62a1c91a859a 585 extern void GetNeutralAccelerations(void);
gke 0:62a1c91a859a 586 extern void GetAccelerations(void);
gke 0:62a1c91a859a 587 extern void AccelerometerTest(void);
gke 0:62a1c91a859a 588 extern void InitAccelerometers(void);
gke 0:62a1c91a859a 589
gke 0:62a1c91a859a 590 // ADXL345 3-Axis Accelerometer
gke 0:62a1c91a859a 591
gke 0:62a1c91a859a 592 #ifdef SIX_DOF
gke 0:62a1c91a859a 593 #define ADXL345_ID 0xA6
gke 0:62a1c91a859a 594 #else
gke 0:62a1c91a859a 595 #define ADXL345_ID 0x53
gke 0:62a1c91a859a 596 #endif // 6DOF
gke 0:62a1c91a859a 597
gke 1:1e3318a30ddd 598 #define ADXL345_WR ADXL345_ID
gke 1:1e3318a30ddd 599 #define ADXL345_RD (ADXL345_ID+1)
gke 0:62a1c91a859a 600
gke 0:62a1c91a859a 601 extern const float GRAVITY_ADXL345;
gke 0:62a1c91a859a 602
gke 0:62a1c91a859a 603 extern void ReadADXL345Acc(void);
gke 0:62a1c91a859a 604 extern void InitADXL345Acc(void);
gke 0:62a1c91a859a 605 extern boolean ADXL345AccActive(void);
gke 0:62a1c91a859a 606
gke 0:62a1c91a859a 607 // LIS3LV02DG 3-Axis Accelerometer 400KHz
gke 0:62a1c91a859a 608
gke 0:62a1c91a859a 609 extern const float GRAVITY_LISL;
gke 0:62a1c91a859a 610
gke 0:62a1c91a859a 611 #define LISL_WHOAMI 0x0f
gke 0:62a1c91a859a 612 #define LISL_OFFSET_X 0x16
gke 0:62a1c91a859a 613 #define LISL_OFFSET_Y 0x17
gke 0:62a1c91a859a 614 #define LISL_OFFSET_Z 0x18
gke 0:62a1c91a859a 615 #define LISL_GAIN_X 0x19
gke 0:62a1c91a859a 616 #define LISL_GAIN_Y 0x1A
gke 0:62a1c91a859a 617 #define LISL_GAIN_Z 0x1B
gke 0:62a1c91a859a 618 #define LISL_CTRLREG_1 0x20
gke 0:62a1c91a859a 619 #define LISL_CTRLREG_2 0x21
gke 0:62a1c91a859a 620 #define LISL_CTRLREG_3 0x22
gke 0:62a1c91a859a 621 #define LISL_STATUS 0x27
gke 0:62a1c91a859a 622 #define LISL_OUTX_L 0x28
gke 0:62a1c91a859a 623 #define LISL_OUTX_H 0x29
gke 0:62a1c91a859a 624 #define LISL_OUTY_L 0x2A
gke 0:62a1c91a859a 625 #define LISL_OUTY_H 0x2B
gke 0:62a1c91a859a 626 #define LISL_OUTZ_L 0x2C
gke 0:62a1c91a859a 627 #define LISL_OUTZ_H 0x2D
gke 0:62a1c91a859a 628 #define LISL_FF_CFG 0x30
gke 0:62a1c91a859a 629 #define LISL_FF_SRC 0x31
gke 0:62a1c91a859a 630 #define LISL_FF_ACK 0x32
gke 0:62a1c91a859a 631 #define LISL_FF_THS_L 0x34
gke 0:62a1c91a859a 632 #define LISL_FF_THS_H 0x35
gke 0:62a1c91a859a 633 #define LISL_FF_DUR 0x36
gke 0:62a1c91a859a 634 #define LISL_DD_CFG 0x38
gke 0:62a1c91a859a 635 #define LISL_INCR_ADDR 0x40
gke 0:62a1c91a859a 636 #define LISL_READ 0x80
gke 0:62a1c91a859a 637 #define LISL_ID 0x3a
gke 0:62a1c91a859a 638
gke 0:62a1c91a859a 639 extern void WriteLISL(uint8, uint8);
gke 0:62a1c91a859a 640 extern void ReadLISLAcc(void);
gke 0:62a1c91a859a 641 extern boolean LISLAccActive(void);
gke 0:62a1c91a859a 642
gke 0:62a1c91a859a 643 // other accelerometers
gke 0:62a1c91a859a 644
gke 0:62a1c91a859a 645 extern real32 Vel[3], Acc[3], AccNeutral[3];
gke 0:62a1c91a859a 646 extern int16 NewAccNeutral[3];
gke 0:62a1c91a859a 647 extern uint8 AccelerometerType;
gke 0:62a1c91a859a 648
gke 0:62a1c91a859a 649 //______________________________________________________________________________________________
gke 0:62a1c91a859a 650
gke 0:62a1c91a859a 651 // analog.c
gke 0:62a1c91a859a 652
gke 1:1e3318a30ddd 653 extern real32 ADC(uint8);
gke 0:62a1c91a859a 654 extern void GetBattery(void);
gke 0:62a1c91a859a 655 extern void BatteryTest(void);
gke 0:62a1c91a859a 656 extern void InitBattery(void);
gke 0:62a1c91a859a 657
gke 0:62a1c91a859a 658 extern void GetRangefinderAltitude(void);
gke 0:62a1c91a859a 659 extern void InitRangefinder(void);
gke 0:62a1c91a859a 660
gke 0:62a1c91a859a 661 extern real32 BatteryVolts, BatteryCurrentADCEstimated, BatteryChargeUsedAH;
gke 0:62a1c91a859a 662 extern real32 BatteryCharge, BatteryCurrent;
gke 0:62a1c91a859a 663 extern real32 BatteryVoltsScale;
gke 0:62a1c91a859a 664
gke 0:62a1c91a859a 665 extern real32 RangefinderAltitude;
gke 0:62a1c91a859a 666
gke 0:62a1c91a859a 667 //______________________________________________________________________________________________
gke 0:62a1c91a859a 668
gke 0:62a1c91a859a 669 // attitude.c
gke 0:62a1c91a859a 670
gke 0:62a1c91a859a 671 enum AttitudeMethods { PremerlaniDCM, MadgwickIMU, MadgwickAHRS};
gke 0:62a1c91a859a 672
gke 0:62a1c91a859a 673 extern void GetAttitude(void);
gke 0:62a1c91a859a 674 extern void DoLegacyYawComp(void);
gke 0:62a1c91a859a 675 extern void AttitudeTest(void);
gke 0:62a1c91a859a 676 extern void InitAttitude(void);
gke 0:62a1c91a859a 677
gke 0:62a1c91a859a 678 extern real32 dT, dTR;
gke 0:62a1c91a859a 679 extern uint32 PrevDCMUpdate;
gke 0:62a1c91a859a 680 extern uint8 AttitudeMethod;
gke 0:62a1c91a859a 681
gke 0:62a1c91a859a 682 // DCM Premerlani
gke 0:62a1c91a859a 683
gke 0:62a1c91a859a 684 extern void DCMNormalise(void);
gke 0:62a1c91a859a 685 extern void DCMDriftCorrection(void);
gke 0:62a1c91a859a 686 extern void AccAdjust(void);
gke 0:62a1c91a859a 687 extern void DCMMotionCompensation(void);
gke 0:62a1c91a859a 688 extern void DCMUpdate(void);
gke 0:62a1c91a859a 689 extern void DCMEulerAngles(void);
gke 0:62a1c91a859a 690
gke 0:62a1c91a859a 691 extern real32 RollPitchError[3];
gke 0:62a1c91a859a 692 extern real32 AccV[3];
gke 0:62a1c91a859a 693 extern real32 GyroV[3];
gke 0:62a1c91a859a 694 extern real32 OmegaV[3];
gke 0:62a1c91a859a 695 extern real32 OmegaP[3];
gke 0:62a1c91a859a 696 extern real32 OmegaI[3];
gke 0:62a1c91a859a 697 extern real32 Omega[3];
gke 0:62a1c91a859a 698 extern real32 DCM[3][3];
gke 0:62a1c91a859a 699 extern real32 U[3][3];
gke 0:62a1c91a859a 700 extern real32 TempM[3][3];
gke 0:62a1c91a859a 701
gke 0:62a1c91a859a 702 // IMU & AHRS S.O.H. Madgwick
gke 0:62a1c91a859a 703
gke 0:62a1c91a859a 704 extern void IMUupdate(real32, real32, real32, real32, real32, real32);
gke 0:62a1c91a859a 705 extern void AHRSupdate(real32, real32, real32, real32, real32, real32, real32, real32, real32);
gke 0:62a1c91a859a 706 extern void EulerAngles(void);
gke 0:62a1c91a859a 707
gke 0:62a1c91a859a 708 extern real32 q0, q1, q2, q3; // quaternion elements representing the estimated orientation
gke 0:62a1c91a859a 709
gke 0:62a1c91a859a 710 //______________________________________________________________________________________________
gke 0:62a1c91a859a 711
gke 0:62a1c91a859a 712 // autonomous.c
gke 0:62a1c91a859a 713
gke 0:62a1c91a859a 714 extern void DoShutdown(void);
gke 0:62a1c91a859a 715 extern void FailsafeHoldPosition(void);
gke 0:62a1c91a859a 716 extern void DoPolarOrientation(void);
gke 0:62a1c91a859a 717 extern void Navigate(int32, int32);
gke 0:62a1c91a859a 718 extern void SetDesiredAltitude(int16);
gke 0:62a1c91a859a 719 extern void DoFailsafeLanding(void);
gke 0:62a1c91a859a 720 extern void AcquireHoldPosition(void);
gke 0:62a1c91a859a 721 extern void NavGainSchedule(int16);
gke 0:62a1c91a859a 722 extern void DoNavigation(void);
gke 0:62a1c91a859a 723 extern void FakeFlight(void);
gke 0:62a1c91a859a 724 extern void DoPPMFailsafe(void);
gke 0:62a1c91a859a 725 extern void WriteWayPointPX(uint8, int32, int32, int16, uint8);
gke 0:62a1c91a859a 726 extern void UAVXNavCommand(void);
gke 0:62a1c91a859a 727 extern void GetWayPointPX(int8);
gke 0:62a1c91a859a 728 extern void InitNavigation(void);
gke 0:62a1c91a859a 729
gke 0:62a1c91a859a 730 typedef struct {
gke 0:62a1c91a859a 731 int32 E, N;
gke 0:62a1c91a859a 732 int16 A;
gke 0:62a1c91a859a 733 uint8 L;
gke 0:62a1c91a859a 734 } WayPoint;
gke 0:62a1c91a859a 735
gke 0:62a1c91a859a 736 enum NavStates { HoldingStation, ReturningHome, AtHome, Descending, Touchdown, Navigating, Loitering};
gke 0:62a1c91a859a 737 enum FailStates { MonitoringRx, Aborting, Terminating, Terminated };
gke 0:62a1c91a859a 738
gke 0:62a1c91a859a 739 extern real32 NavCorr[3], NavCorrp[3];
gke 0:62a1c91a859a 740 extern real32 NavE[3], NavEp[3], NavIntE[3];
gke 0:62a1c91a859a 741 extern int16 NavYCorrLimit;
gke 0:62a1c91a859a 742
gke 0:62a1c91a859a 743 extern int8 FailState;
gke 0:62a1c91a859a 744 extern WayPoint WP;
gke 0:62a1c91a859a 745 extern int8 CurrWP;
gke 0:62a1c91a859a 746 extern int8 NoOfWayPoints;
gke 0:62a1c91a859a 747 extern int16 WPAltitude;
gke 0:62a1c91a859a 748 extern int32 WPLatitude, WPLongitude;
gke 0:62a1c91a859a 749
gke 0:62a1c91a859a 750 extern real32 WayHeading;
gke 0:62a1c91a859a 751 extern real32 NavPolarRadius, NavNeutralRadius, NavProximityRadius, NavProximityAltitude;
gke 0:62a1c91a859a 752
gke 0:62a1c91a859a 753 extern int24 NavRTHTimeoutmS;
gke 0:62a1c91a859a 754 extern int8 NavState;
gke 0:62a1c91a859a 755 extern int16 NavSensitivity, RollPitchMax;
gke 0:62a1c91a859a 756 extern int16 AltSum;
gke 0:62a1c91a859a 757
gke 0:62a1c91a859a 758 extern int16 EffNavSensitivity;
gke 0:62a1c91a859a 759 extern int16 EastP, EastDiffSum, EastI, EastCorr, NorthP, NorthDiffSum, NorthI, NorthCorr;
gke 0:62a1c91a859a 760 extern int24 EastD, EastDiffP, NorthD, NorthDiffP;
gke 0:62a1c91a859a 761
gke 0:62a1c91a859a 762 //______________________________________________________________________________________________
gke 0:62a1c91a859a 763
gke 0:62a1c91a859a 764 // baro.c
gke 0:62a1c91a859a 765
gke 0:62a1c91a859a 766 #define BARO_INIT_RETRIES 10 // max number of initialisation retries
gke 0:62a1c91a859a 767
gke 0:62a1c91a859a 768 enum BaroTypes { BaroBMP085, BaroSMD500, BaroMPX4115, BaroUnknown };
gke 0:62a1c91a859a 769
gke 0:62a1c91a859a 770 #define ADS7823_TIME_MS 50 // 20Hz
gke 0:62a1c91a859a 771 #define ADS7823_MAX 4095 // 12 bits
gke 0:62a1c91a859a 772 #define ADS7823_ID 0x90 // ADS7823 ADC
gke 1:1e3318a30ddd 773 #define ADS7823_WR ADS7823_ID // ADS7823 ADC
gke 1:1e3318a30ddd 774 #define ADS7823_RD (ADS7823_ID+1) // ADS7823 ADC
gke 0:62a1c91a859a 775 #define ADS7823_CMD 0x00
gke 0:62a1c91a859a 776
gke 1:1e3318a30ddd 777 extern uint8 MCP4725_ID_Actual;
gke 1:1e3318a30ddd 778
gke 0:62a1c91a859a 779 #define MCP4725_MAX 4095 // 12 bits
gke 1:1e3318a30ddd 780 #define MCP4725_ID_0xC8 0xc8
gke 1:1e3318a30ddd 781 #define MCP4725_ID_0xCC 0xcc
gke 0:62a1c91a859a 782 #define MCP4725_CMD 0x40 // write to DAC registor in next 2 bytes
gke 0:62a1c91a859a 783 #define MCP4725_EPROM 0x60 // write to DAC registor and eprom
gke 0:62a1c91a859a 784
gke 0:62a1c91a859a 785 extern void SetFreescaleMCP4725(int16);
gke 0:62a1c91a859a 786 extern void SetFreescaleOffset(void);
gke 0:62a1c91a859a 787 extern void ReadFreescaleBaro(void);
gke 0:62a1c91a859a 788 extern real32 FreescaleToDM(int24);
gke 0:62a1c91a859a 789 extern void GetFreescaleBaroAltitude(void);
gke 0:62a1c91a859a 790 extern boolean IsFreescaleBaroActive(void);
gke 1:1e3318a30ddd 791 extern boolean IdentifyMCP4725(void);
gke 0:62a1c91a859a 792 extern void InitFreescaleBarometer(void);
gke 0:62a1c91a859a 793
gke 0:62a1c91a859a 794 #define BOSCH_ID_BMP085 0x55
gke 0:62a1c91a859a 795 #define BOSCH_ID 0xee
gke 1:1e3318a30ddd 796 #define BOSCH_WR BOSCH_ID_BMP085
gke 1:1e3318a30ddd 797 #define BOSCH_RD (BOSCH_ID_BMP085+1)
gke 0:62a1c91a859a 798 #define BOSCH_TEMP_BMP085 0x2e
gke 0:62a1c91a859a 799 #define BOSCH_TEMP_SMD500 0x6e
gke 0:62a1c91a859a 800 #define BOSCH_PRESS 0xf4
gke 0:62a1c91a859a 801 #define BOSCH_CTL 0xf4 // OSRS=3 for BMP085 25.5mS, SMD500 34mS
gke 0:62a1c91a859a 802 #define BOSCH_ADC_MSB 0xf6
gke 0:62a1c91a859a 803 #define BOSCH_ADC_LSB 0xf7
gke 0:62a1c91a859a 804 #define BOSCH_ADC_XLSB 0xf8 // BMP085
gke 0:62a1c91a859a 805 #define BOSCH_TYPE 0xd0
gke 0:62a1c91a859a 806
gke 0:62a1c91a859a 807 extern void StartBoschBaroADC(boolean);
gke 0:62a1c91a859a 808 extern void ReadBoschBaro(void);
gke 0:62a1c91a859a 809 extern int24 CompensatedBoschPressure(uint16, uint16);
gke 0:62a1c91a859a 810 extern void GetBoschBaroAltitude(void);
gke 0:62a1c91a859a 811 extern boolean IsBoschBaroActive(void);
gke 0:62a1c91a859a 812 extern void InitBoschBarometer(void);
gke 0:62a1c91a859a 813
gke 0:62a1c91a859a 814 extern void GetBaroAltitude(void);
gke 0:62a1c91a859a 815 extern void InitBarometer(void);
gke 0:62a1c91a859a 816
gke 0:62a1c91a859a 817 extern void ShowBaroType(void);
gke 0:62a1c91a859a 818 extern void BaroTest(void);
gke 0:62a1c91a859a 819
gke 0:62a1c91a859a 820 extern int32 OriginBaroPressure, CompBaroPressure;
gke 0:62a1c91a859a 821 extern uint16 BaroPressure, BaroTemperature;
gke 0:62a1c91a859a 822 extern boolean AcquiringPressure;
gke 0:62a1c91a859a 823 extern real32 BaroRelAltitude, BaroRelAltitudeP;
gke 0:62a1c91a859a 824 extern i16u BaroVal;
gke 0:62a1c91a859a 825 extern int8 BaroType;
gke 0:62a1c91a859a 826 extern int16 AltitudeUpdateRate;
gke 0:62a1c91a859a 827 extern int8 BaroRetries;
gke 0:62a1c91a859a 828
gke 0:62a1c91a859a 829 extern real32 FakeBaroRelAltitude;
gke 0:62a1c91a859a 830
gke 0:62a1c91a859a 831 //______________________________________________________________________________________________
gke 0:62a1c91a859a 832
gke 0:62a1c91a859a 833 // compass.c
gke 0:62a1c91a859a 834
gke 0:62a1c91a859a 835 enum CompassTypes { HMC5843, HMC6352, NoCompass };
gke 0:62a1c91a859a 836
gke 0:62a1c91a859a 837 #define COMPASS_UPDATE_MS 50
gke 0:62a1c91a859a 838 #define COMPASS_UPDATE_S (real32)(COMPASS_UPDATE_MS * 0.001)
gke 0:62a1c91a859a 839 #define COMPASS_FREQ 10 // Hz must be less than 10Hz
gke 0:62a1c91a859a 840
gke 0:62a1c91a859a 841 #define COMPASS_MAXDEV 30 // maximum yaw compensation of compass heading
gke 0:62a1c91a859a 842 #define COMPASS_MIDDLE 10 // yaw stick neutral dead zone
gke 0:62a1c91a859a 843
gke 0:62a1c91a859a 844 const real32 OneOnTwoPiCompassF = ( 1.0 / ( TWOPI * COMPASS_FREQ ));
gke 0:62a1c91a859a 845
gke 0:62a1c91a859a 846 extern void ReadCompass(void);
gke 0:62a1c91a859a 847 extern void GetHeading(void);
gke 0:62a1c91a859a 848 extern void ShowCompassType(void);
gke 0:62a1c91a859a 849 extern void DoCompassTest(void);
gke 0:62a1c91a859a 850 extern void CalibrateCompass(void);
gke 0:62a1c91a859a 851 extern void InitCompass(void);
gke 0:62a1c91a859a 852
gke 0:62a1c91a859a 853 // HMC5843 Compass
gke 0:62a1c91a859a 854
gke 0:62a1c91a859a 855 #define HMC5843_ID 0x3C // 0x1E 9DOF
gke 1:1e3318a30ddd 856 #define HMC5843_WR HMC5843_ID
gke 1:1e3318a30ddd 857 #define HMC5843_RD (HMC5843_ID+1)
gke 0:62a1c91a859a 858
gke 0:62a1c91a859a 859 extern void ReadHMC5843(void);
gke 0:62a1c91a859a 860 extern void GetHMC5843Parameters(void);
gke 0:62a1c91a859a 861 extern void DoHMC5843Test(void);
gke 0:62a1c91a859a 862 extern void CalibrateHMC5843(void);
gke 0:62a1c91a859a 863 extern void InitHMC5843(void);
gke 0:62a1c91a859a 864 extern boolean IsHMC5843Active(void);
gke 0:62a1c91a859a 865
gke 0:62a1c91a859a 866 // HMC6352
gke 0:62a1c91a859a 867
gke 1:1e3318a30ddd 868 #define HMC6352_ID 0x42
gke 1:1e3318a30ddd 869 #define HMC6352_WR HMC6352_ID
gke 1:1e3318a30ddd 870 #define HMC6352_RD (HMC6352_ID+1)
gke 0:62a1c91a859a 871
gke 0:62a1c91a859a 872 extern void ReadHMC6352(void);
gke 0:62a1c91a859a 873 extern uint8 WriteByteHMC6352(uint8);
gke 0:62a1c91a859a 874 extern void GetHMC6352Parameters(void);
gke 0:62a1c91a859a 875 extern void DoHMC6352Test(void);
gke 0:62a1c91a859a 876 extern void CalibrateHMC6352(void);
gke 0:62a1c91a859a 877 extern void InitHMC6352(void);
gke 0:62a1c91a859a 878 extern boolean HMC6352Active(void);
gke 0:62a1c91a859a 879
gke 1:1e3318a30ddd 880 typedef struct {
gke 1:1e3318a30ddd 881 real32 V;
gke 1:1e3318a30ddd 882 real32 Offset;
gke 1:1e3318a30ddd 883 } MagStruct;
gke 0:62a1c91a859a 884 extern MagStruct Mag[3];
gke 0:62a1c91a859a 885 extern real32 MagDeviation, CompassOffset;
gke 0:62a1c91a859a 886 extern real32 MagHeading, Heading, FakeHeading;
gke 0:62a1c91a859a 887 extern real32 HeadingSin, HeadingCos;
gke 0:62a1c91a859a 888 extern uint8 CompassType;
gke 0:62a1c91a859a 889
gke 0:62a1c91a859a 890 //______________________________________________________________________________________________
gke 0:62a1c91a859a 891
gke 0:62a1c91a859a 892 // control.c
gke 0:62a1c91a859a 893
gke 1:1e3318a30ddd 894 extern real32 PTerm, ITerm, DTerm; //zzz
gke 1:1e3318a30ddd 895
gke 0:62a1c91a859a 896 extern void DoAltitudeHold(void);
gke 0:62a1c91a859a 897 extern void UpdateAltitudeSource(void);
gke 0:62a1c91a859a 898 extern void AltitudeHold(void);
gke 0:62a1c91a859a 899
gke 0:62a1c91a859a 900 extern void LimitYawSum(void);
gke 0:62a1c91a859a 901 extern void InertialDamping(void);
gke 0:62a1c91a859a 902 extern void DoOrientationTransform(void);
gke 0:62a1c91a859a 903 extern void DoControl(void);
gke 0:62a1c91a859a 904
gke 0:62a1c91a859a 905 extern void LightsAndSirens(void);
gke 0:62a1c91a859a 906 extern void InitControl(void);
gke 0:62a1c91a859a 907
gke 0:62a1c91a859a 908 extern real32 Angle[3], Anglep[3], Rate[3], Ratep[3];
gke 0:62a1c91a859a 909 extern real32 Comp[4];
gke 0:62a1c91a859a 910 extern real32 DescentComp;
gke 0:62a1c91a859a 911 extern real32 Rl, Pl, Yl, Ylp;
gke 0:62a1c91a859a 912 extern real32 GS;
gke 0:62a1c91a859a 913
gke 0:62a1c91a859a 914 extern int16 HoldYaw, YawSlewLimit;
gke 0:62a1c91a859a 915 extern int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
gke 0:62a1c91a859a 916 extern int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
gke 0:62a1c91a859a 917 extern real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
gke 1:1e3318a30ddd 918 extern real32 CameraRollAngle, CameraPitchAngle;
gke 0:62a1c91a859a 919 extern int16 CurrMaxRollPitch;
gke 0:62a1c91a859a 920
gke 0:62a1c91a859a 921 extern int16 AttitudeHoldResetCount;
gke 0:62a1c91a859a 922 extern real32 AltDiffSum, AltD, AltDSum;
gke 0:62a1c91a859a 923 extern real32 DesiredAltitude, Altitude, AltitudeP;
gke 0:62a1c91a859a 924 extern real32 ROC;
gke 0:62a1c91a859a 925 extern boolean FirstPass;
gke 0:62a1c91a859a 926
gke 0:62a1c91a859a 927 extern uint32 AltuSp;
gke 0:62a1c91a859a 928 extern int16 DescentLimiter;
gke 0:62a1c91a859a 929
gke 0:62a1c91a859a 930 extern int16 FakeDesiredPitch, FakeDesiredRoll, FakeDesiredYaw;
gke 0:62a1c91a859a 931
gke 0:62a1c91a859a 932 //______________________________________________________________________________________________
gke 0:62a1c91a859a 933
gke 0:62a1c91a859a 934 // gps.c
gke 0:62a1c91a859a 935
gke 0:62a1c91a859a 936 extern void UpdateField(void);
gke 0:62a1c91a859a 937 extern int32 ConvertGPSToM(int32);
gke 0:62a1c91a859a 938 extern int32 ConvertMToGPS(int32);
gke 0:62a1c91a859a 939 extern int24 ConvertInt(uint8, uint8);
gke 0:62a1c91a859a 940 extern real32 ConvertReal(uint8, uint8);
gke 0:62a1c91a859a 941 extern int32 ConvertLatLonM(uint8, uint8);
gke 0:62a1c91a859a 942 extern void ConvertUTime(uint8, uint8);
gke 0:62a1c91a859a 943 extern void ConvertUDate(uint8, uint8);
gke 0:62a1c91a859a 944 extern void ParseGPGGASentence(void);
gke 0:62a1c91a859a 945 extern void ParseGPRMCSentence(void);
gke 0:62a1c91a859a 946 extern void SetGPSOrigin(void);
gke 0:62a1c91a859a 947 extern void ParseGPSSentence(void);
gke 0:62a1c91a859a 948 extern void RxGPSPacket(uint8);
gke 0:62a1c91a859a 949 extern void SetGPSOrigin(void);
gke 0:62a1c91a859a 950 extern void DoGPS(void);
gke 0:62a1c91a859a 951 extern void GPSTest(void);
gke 0:62a1c91a859a 952 extern void UpdateGPS(void);
gke 0:62a1c91a859a 953 extern void InitGPS(void);
gke 0:62a1c91a859a 954
gke 0:62a1c91a859a 955 #define MAXTAGINDEX 4
gke 0:62a1c91a859a 956 #define GPSRXBUFFLENGTH 80
gke 0:62a1c91a859a 957 typedef struct {
gke 0:62a1c91a859a 958 uint8 s[GPSRXBUFFLENGTH];
gke 0:62a1c91a859a 959 uint8 length;
gke 0:62a1c91a859a 960 } NMEAStruct;
gke 0:62a1c91a859a 961
gke 0:62a1c91a859a 962 #define MAX_NMEA_SENTENCES 2
gke 0:62a1c91a859a 963 #define NMEA_TAG_INDEX 4
gke 0:62a1c91a859a 964
gke 0:62a1c91a859a 965 enum GPSPackeType { GPGGAPacketTag, GPRMCPacketTag, GPSUnknownPacketTag };
gke 0:62a1c91a859a 966 extern NMEAStruct NMEA;
gke 0:62a1c91a859a 967 extern const uint8 NMEATags[MAX_NMEA_SENTENCES][5];
gke 0:62a1c91a859a 968
gke 0:62a1c91a859a 969 extern uint8 GPSPacketTag;
gke 0:62a1c91a859a 970 extern tm GPSTime;
gke 0:62a1c91a859a 971 extern int32 GPSStartTime, GPSSeconds;
gke 0:62a1c91a859a 972 extern int32 GPSLatitude, GPSLongitude;
gke 0:62a1c91a859a 973 extern int32 OriginLatitude, OriginLongitude;
gke 0:62a1c91a859a 974 extern real32 GPSAltitude, GPSRelAltitude, GPSOriginAltitude;
gke 0:62a1c91a859a 975 extern int32 DesiredLatitude, DesiredLongitude;
gke 0:62a1c91a859a 976 extern int32 LatitudeP, LongitudeP, HoldLatitude, HoldLongitude;
gke 0:62a1c91a859a 977 extern real32 GPSLongitudeCorrection;
gke 0:62a1c91a859a 978 extern real32 GPSHeading, GPSMagHeading, GPSMagDeviation, GPSVel, GPSVelp, GPSROC;
gke 0:62a1c91a859a 979 extern int8 GPSNoOfSats;
gke 0:62a1c91a859a 980 extern int8 GPSFix;
gke 0:62a1c91a859a 981 extern int16 GPSHDilute;
gke 0:62a1c91a859a 982
gke 0:62a1c91a859a 983 extern real32 GPSdT, GPSdTR;
gke 0:62a1c91a859a 984 extern uint32 GPSuSp;
gke 0:62a1c91a859a 985
gke 0:62a1c91a859a 986 extern int32 FakeGPSLongitude, FakeGPSLatitude;
gke 0:62a1c91a859a 987
gke 0:62a1c91a859a 988 extern uint8 ll, tt, ss, RxCh;
gke 0:62a1c91a859a 989 extern uint8 GPSCheckSumChar, GPSTxCheckSum;
gke 0:62a1c91a859a 990
gke 0:62a1c91a859a 991 //______________________________________________________________________________________________
gke 0:62a1c91a859a 992
gke 0:62a1c91a859a 993 // gyro.c
gke 0:62a1c91a859a 994
gke 0:62a1c91a859a 995 enum GyroTypes { MLX90609Gyro, ADXRS150Gyro, IDG300Gyro, LY530Gyro, ADXRS300Gyro, ITG3200Gyro,
gke 0:62a1c91a859a 996 IRSensors, UnknownGyro
gke 0:62a1c91a859a 997 };
gke 0:62a1c91a859a 998
gke 0:62a1c91a859a 999 extern void ReadGyros(void);
gke 0:62a1c91a859a 1000 extern void GetGyroRates(void);
gke 0:62a1c91a859a 1001 extern void CheckGyroFault(uint8, uint8, uint8);
gke 0:62a1c91a859a 1002 extern void ErectGyros(void);
gke 0:62a1c91a859a 1003 extern void GyroTest(void);
gke 0:62a1c91a859a 1004 extern void InitGyros(void);
gke 0:62a1c91a859a 1005 extern void ShowGyroType(void);
gke 0:62a1c91a859a 1006
gke 0:62a1c91a859a 1007 // Generic Analog Gyrso
gke 0:62a1c91a859a 1008 extern void ReadAnalogGyros(void);
gke 0:62a1c91a859a 1009 extern void InitAnalogGyros(void);
gke 0:62a1c91a859a 1010 extern void CheckAnalogGyroFault(uint8, uint8, uint8);
gke 0:62a1c91a859a 1011 extern void AnalogGyroTest(void);
gke 0:62a1c91a859a 1012
gke 0:62a1c91a859a 1013 // ITG3200 3 Axis Gyro
gke 0:62a1c91a859a 1014
gke 0:62a1c91a859a 1015 #define ITG3200_WHO 0x00
gke 0:62a1c91a859a 1016 #define ITG3200_SMPL 0x15
gke 0:62a1c91a859a 1017 #define ITG3200_DLPF 0x16
gke 0:62a1c91a859a 1018 #define ITG3200_INT_C 0x17
gke 0:62a1c91a859a 1019 #define ITG3200_TMP_H 0x18
gke 0:62a1c91a859a 1020 #define ITG3200_TMP_L 0x1C
gke 0:62a1c91a859a 1021 #define ITG3200_GX_H 0x1D
gke 0:62a1c91a859a 1022 #define ITG3200_GX_L 0x1E
gke 0:62a1c91a859a 1023 #define ITG3200_GY_H 0x1F
gke 0:62a1c91a859a 1024 #define ITG3200_GY_L 0x20
gke 0:62a1c91a859a 1025 #define ITG3200_GZ_H 0x21
gke 0:62a1c91a859a 1026 #define ITG3200_GZ_L 0x22
gke 0:62a1c91a859a 1027 #define ITG3200_PWR_M 0x3E
gke 0:62a1c91a859a 1028
gke 0:62a1c91a859a 1029 #ifdef SIX_DOF
gke 0:62a1c91a859a 1030 #define ITG3200_ID 0xD0
gke 0:62a1c91a859a 1031 #else
gke 0:62a1c91a859a 1032 #define ITG3200_ID 0xD2
gke 0:62a1c91a859a 1033 #endif // 6DOF
gke 0:62a1c91a859a 1034
gke 1:1e3318a30ddd 1035 #define ITG3200_WR ITG3200_ID
gke 1:1e3318a30ddd 1036 #define ITG3200_RD (ITG3200_ID+1)
gke 0:62a1c91a859a 1037
gke 0:62a1c91a859a 1038 extern void ReadITG3200Gyro(void);
gke 0:62a1c91a859a 1039 extern uint8 ReadByteITG3200(uint8);
gke 0:62a1c91a859a 1040 extern void WriteByteITG3200(uint8, uint8);
gke 0:62a1c91a859a 1041 extern void InitITG3200Gyro(void);
gke 0:62a1c91a859a 1042 extern void ITG3200Test(void);
gke 0:62a1c91a859a 1043 extern boolean ITG3200GyroActive(void);
gke 0:62a1c91a859a 1044
gke 0:62a1c91a859a 1045 extern const real32 GyroToRadian[];
gke 0:62a1c91a859a 1046 extern real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
gke 0:62a1c91a859a 1047 extern uint8 GyroType;
gke 0:62a1c91a859a 1048
gke 0:62a1c91a859a 1049 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1050
gke 0:62a1c91a859a 1051 // harness.c
gke 0:62a1c91a859a 1052
gke 0:62a1c91a859a 1053 extern void UpdateRTC(void);
gke 0:62a1c91a859a 1054 extern void InitHarness(void);
gke 0:62a1c91a859a 1055
gke 0:62a1c91a859a 1056 extern LocalFileSystem Flash;
gke 0:62a1c91a859a 1057
gke 0:62a1c91a859a 1058 // 1 GND
gke 0:62a1c91a859a 1059 // 2 4.5-9V
gke 0:62a1c91a859a 1060 // 3 VBat
gke 0:62a1c91a859a 1061 // 4 NReset
gke 0:62a1c91a859a 1062
gke 0:62a1c91a859a 1063 //extern SPI SPI0; // 5 SPI MOSI, 6 SPI MOSO, 7 SPI CLK
gke 0:62a1c91a859a 1064 //extern DigitalOut SPICS; // 8
gke 0:62a1c91a859a 1065 extern SDFileSystem SDCard;
gke 0:62a1c91a859a 1066
gke 0:62a1c91a859a 1067 //extern I2C I2C1; // 9, 10
gke 0:62a1c91a859a 1068 extern SerialBuffered TelemetrySerial;
gke 0:62a1c91a859a 1069 extern DigitalIn Armed; // 11 SPI MOSI
gke 0:62a1c91a859a 1070 extern DigitalOut PWMCamPitch; // 12 SPI MOSO // 12 SPI MOSO
gke 0:62a1c91a859a 1071 extern Serial GPSSerial; // 13 Tx1 / SPI CLK, 14 Rx1
gke 0:62a1c91a859a 1072
gke 0:62a1c91a859a 1073 extern AnalogIn PitchADC; // 15 AN0
gke 0:62a1c91a859a 1074 extern AnalogIn RollADC; // 16 AN1
gke 0:62a1c91a859a 1075 extern AnalogIn YawADC; // 17 AN2
gke 0:62a1c91a859a 1076
gke 0:62a1c91a859a 1077 extern AnalogIn RangefinderADC; // 18 AN3
gke 0:62a1c91a859a 1078 extern AnalogIn BatteryCurrentADC; // 19 AN4
gke 0:62a1c91a859a 1079 extern AnalogIn BatteryVoltsADC; // 20 AN5
gke 0:62a1c91a859a 1080
gke 1:1e3318a30ddd 1081 enum ADCValues { ADCPitch, ADCRoll, ADCYaw, ADCRangefinder, ADCBatteryCurrent, ADCBatteryVolts };
gke 1:1e3318a30ddd 1082
gke 0:62a1c91a859a 1083 extern PwmOut Out0; // 21
gke 0:62a1c91a859a 1084 extern PwmOut Out1; // 22
gke 0:62a1c91a859a 1085 extern PwmOut Out2; // 23
gke 0:62a1c91a859a 1086 extern PwmOut Out3; // 24
gke 0:62a1c91a859a 1087
gke 0:62a1c91a859a 1088 //extern PwmOut Out4; // 25
gke 0:62a1c91a859a 1089 //extern PwmOut Out5; // 26
gke 1:1e3318a30ddd 1090 extern DigitalOut DebugPin; // 25
gke 1:1e3318a30ddd 1091
gke 1:1e3318a30ddd 1092 #ifdef SW_I2C
gke 1:1e3318a30ddd 1093
gke 1:1e3318a30ddd 1094 class MyI2C {
gke 1:1e3318a30ddd 1095
gke 1:1e3318a30ddd 1096 private:
gke 1:1e3318a30ddd 1097
gke 1:1e3318a30ddd 1098 boolean waitclock(void);
gke 1:1e3318a30ddd 1099
gke 1:1e3318a30ddd 1100 public:
gke 1:1e3318a30ddd 1101
gke 1:1e3318a30ddd 1102 void frequency(uint32 f);
gke 1:1e3318a30ddd 1103 void start(void);
gke 1:1e3318a30ddd 1104 void stop(void);
gke 1:1e3318a30ddd 1105 uint8 blockread(uint8 r, char* b, uint8);
gke 1:1e3318a30ddd 1106 uint8 read(uint8 r);
gke 1:1e3318a30ddd 1107 void blockwrite(uint8 a, const char* b, uint8 l);
gke 1:1e3318a30ddd 1108 uint8 write(uint8 d);
gke 1:1e3318a30ddd 1109 };
gke 1:1e3318a30ddd 1110
gke 1:1e3318a30ddd 1111 extern MyI2C I2C0; // 27, 28
gke 1:1e3318a30ddd 1112 extern DigitalInOut I2C0SCL;
gke 1:1e3318a30ddd 1113 extern DigitalInOut I2C0SDA;
gke 1:1e3318a30ddd 1114 #else
gke 0:62a1c91a859a 1115
gke 0:62a1c91a859a 1116 extern I2C I2C0; // 27, 28
gke 1:1e3318a30ddd 1117 #define blockread read
gke 1:1e3318a30ddd 1118 #define blockwrite write
gke 1:1e3318a30ddd 1119
gke 1:1e3318a30ddd 1120 #endif // SW_I2C
gke 0:62a1c91a859a 1121
gke 0:62a1c91a859a 1122 extern DigitalIn RCIn; // 29 CAN
gke 0:62a1c91a859a 1123 extern DigitalOut PWMCamRoll; // 30 CAN
gke 0:62a1c91a859a 1124
gke 0:62a1c91a859a 1125 //extern Serial TelemetrySerial;
gke 0:62a1c91a859a 1126 // 31 USB +, 32 USB -
gke 0:62a1c91a859a 1127 // 34 -37 Ethernet
gke 0:62a1c91a859a 1128 // 38 IF +
gke 0:62a1c91a859a 1129 // 39 IF -
gke 0:62a1c91a859a 1130 // 40 3.3V Out
gke 0:62a1c91a859a 1131
gke 0:62a1c91a859a 1132 extern DigitalOut BlueLED;
gke 0:62a1c91a859a 1133 extern DigitalOut GreenLED;
gke 0:62a1c91a859a 1134 extern DigitalOut RedLED;
gke 0:62a1c91a859a 1135 extern DigitalOut YellowLED;
gke 0:62a1c91a859a 1136
gke 0:62a1c91a859a 1137 extern InterruptIn RCInterrupt;
gke 0:62a1c91a859a 1138
gke 0:62a1c91a859a 1139 extern char RTCString[], RTCLogfile[];
gke 0:62a1c91a859a 1140 extern struct tm* RTCTime;
gke 0:62a1c91a859a 1141
gke 0:62a1c91a859a 1142 #define I2CTEMP I2C0
gke 0:62a1c91a859a 1143 #define I2CBARO I2C0
gke 1:1e3318a30ddd 1144 #define I2CBAROAddressResponds I2C0AddressResponds
gke 0:62a1c91a859a 1145 #define I2CGYRO I2C0
gke 1:1e3318a30ddd 1146 #define I2CGYROAddressResponds I2C0AddressResponds
gke 0:62a1c91a859a 1147 #define I2CACC I2C0
gke 1:1e3318a30ddd 1148 #define I2CACCAddressResponds I2C0AddressResponds
gke 0:62a1c91a859a 1149 #define I2CCOMPASS I2C0
gke 1:1e3318a30ddd 1150 #define I2CCOMPASSAddressResponds I2C0AddressResponds
gke 0:62a1c91a859a 1151 #define I2CESC I2C0
gke 0:62a1c91a859a 1152 #define I2CLED I2C0
gke 0:62a1c91a859a 1153
gke 0:62a1c91a859a 1154 #define SPIACC SPI0
gke 0:62a1c91a859a 1155
gke 0:62a1c91a859a 1156 #define mSClock timer.read_ms
gke 0:62a1c91a859a 1157 #define uSClock timer.read_us
gke 0:62a1c91a859a 1158
gke 0:62a1c91a859a 1159 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1160
gke 0:62a1c91a859a 1161 // irq.c
gke 0:62a1c91a859a 1162
gke 0:62a1c91a859a 1163 #define CONTROLS 7
gke 0:62a1c91a859a 1164 #define MAX_CONTROLS 12 // maximum Rx channels
gke 0:62a1c91a859a 1165
gke 0:62a1c91a859a 1166 #define RxFilter MediumFilterU
gke 0:62a1c91a859a 1167 //#define RxFilter SoftFilterU
gke 0:62a1c91a859a 1168 //#define RxFilter NoFilter
gke 0:62a1c91a859a 1169
gke 0:62a1c91a859a 1170 #define RC_GOOD_BUCKET_MAX 20
gke 0:62a1c91a859a 1171 #define RC_GOOD_RATIO 4
gke 0:62a1c91a859a 1172
gke 0:62a1c91a859a 1173 #define RC_MINIMUM 0
gke 0:62a1c91a859a 1174
gke 0:62a1c91a859a 1175 #define RC_MAXIMUM 238
gke 0:62a1c91a859a 1176
gke 0:62a1c91a859a 1177 #define RC_NEUTRAL ((RC_MAXIMUM-RC_MINIMUM+1)/2)
gke 0:62a1c91a859a 1178
gke 0:62a1c91a859a 1179 #define RC_MAX_ROLL_PITCH (170)
gke 0:62a1c91a859a 1180
gke 0:62a1c91a859a 1181 #define RC_THRES_STOP ((6L*RC_MAXIMUM)/100)
gke 0:62a1c91a859a 1182 #define RC_THRES_START ((10L*RC_MAXIMUM)/100)
gke 0:62a1c91a859a 1183
gke 0:62a1c91a859a 1184 #define RC_FRAME_TIMEOUT_MS 25
gke 0:62a1c91a859a 1185 #define RC_SIGNAL_TIMEOUT_MS (5L*RC_FRAME_TIMEOUT_MS)
gke 0:62a1c91a859a 1186 #define RC_THR_MAX RC_MAXIMUM
gke 0:62a1c91a859a 1187
gke 0:62a1c91a859a 1188 #define MAX_ROLL_PITCH RC_NEUTRAL // Rx stick units - rely on Tx Rate/Exp
gke 0:62a1c91a859a 1189
gke 0:62a1c91a859a 1190 #ifdef RX6CH
gke 0:62a1c91a859a 1191 #define RC_CONTROLS 5
gke 0:62a1c91a859a 1192 #else
gke 0:62a1c91a859a 1193 #define RC_CONTROLS CONTROLS
gke 0:62a1c91a859a 1194 #endif //RX6CH
gke 0:62a1c91a859a 1195
gke 0:62a1c91a859a 1196 extern void InitTimersAndInterrupts(void);
gke 0:62a1c91a859a 1197
gke 0:62a1c91a859a 1198 extern void enableTxIrq0(void);
gke 0:62a1c91a859a 1199 extern void disableTxIrq0(void);
gke 0:62a1c91a859a 1200 extern void enableTxIrq1(void);
gke 0:62a1c91a859a 1201 extern void disableTxIrq1(void);
gke 0:62a1c91a859a 1202
gke 0:62a1c91a859a 1203 extern void RCISR(void);
gke 0:62a1c91a859a 1204 extern void RCNullISR(void);
gke 0:62a1c91a859a 1205 extern void RCTimeoutISR(void);
gke 0:62a1c91a859a 1206 extern void GPSInISR(void);
gke 0:62a1c91a859a 1207 extern void GPSOutISR(void);
gke 0:62a1c91a859a 1208 extern void TelemetryInISR(void);
gke 0:62a1c91a859a 1209 extern void TelemetryOutISR(void);
gke 0:62a1c91a859a 1210 extern void RazorInISR(void);
gke 0:62a1c91a859a 1211 extern void RazorOutISR(void);
gke 0:62a1c91a859a 1212
gke 0:62a1c91a859a 1213 enum { Clock, GeneralCountdown, UpdateTimeout, RCSignalTimeout, BeeperTimeout, ThrottleIdleTimeout,
gke 0:62a1c91a859a 1214 FailsafeTimeout, AbortTimeout, RxFailsafeTimeout, DescentUpdate, StickChangeUpdate, NavStateTimeout, LastValidRx,
gke 0:62a1c91a859a 1215 LastGPS, StartTime, GPSTimeout, LEDChaserUpdate, LastBattery, TelemetryUpdate, NavActiveTime, BeeperUpdate,
gke 0:62a1c91a859a 1216 ArmedTimeout, ThrottleUpdate, RazorUpdate, VerticalDampingUpdate, CamUpdate, BaroUpdate, CompassUpdate
gke 0:62a1c91a859a 1217 };
gke 0:62a1c91a859a 1218
gke 0:62a1c91a859a 1219 enum WaitPacketStates { WaitSentinel, WaitTag, WaitBody, WaitCheckSum};
gke 0:62a1c91a859a 1220
gke 0:62a1c91a859a 1221 extern uint32 mS[];
gke 0:62a1c91a859a 1222 extern int16 PPM[];
gke 0:62a1c91a859a 1223 extern int8 PPM_Index;
gke 0:62a1c91a859a 1224 extern uint32 PrevEdge, CurrEdge;
gke 0:62a1c91a859a 1225 extern uint8 Intersection, PrevPattern, CurrPattern;
gke 0:62a1c91a859a 1226 extern uint32 Width;
gke 0:62a1c91a859a 1227 extern uint8 RxState;
gke 0:62a1c91a859a 1228 extern boolean WaitingForSync;
gke 0:62a1c91a859a 1229
gke 0:62a1c91a859a 1230 extern int8 SignalCount;
gke 0:62a1c91a859a 1231 extern uint16 RCGlitches;
gke 0:62a1c91a859a 1232
gke 0:62a1c91a859a 1233 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1234
gke 0:62a1c91a859a 1235 // ir.c
gke 0:62a1c91a859a 1236
gke 0:62a1c91a859a 1237 extern void GetIRAttitude(void);
gke 0:62a1c91a859a 1238 extern void TrackIRMaxMin(uint8);
gke 0:62a1c91a859a 1239 extern void InitIRSensors(void);
gke 0:62a1c91a859a 1240
gke 0:62a1c91a859a 1241 extern real32 IR[3], IRMax, IRMin, IRSwing;
gke 0:62a1c91a859a 1242
gke 0:62a1c91a859a 1243 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1244
gke 0:62a1c91a859a 1245 // i2c.c
gke 0:62a1c91a859a 1246
gke 1:1e3318a30ddd 1247 #ifdef SW_I2C
gke 1:1e3318a30ddd 1248 #define I2C_ACK 0
gke 1:1e3318a30ddd 1249 #define I2C_NACK 1
gke 1:1e3318a30ddd 1250 #else
gke 0:62a1c91a859a 1251 #define I2C_ACK 1
gke 0:62a1c91a859a 1252 #define I2C_NACK 0
gke 1:1e3318a30ddd 1253 #endif // SW_I2C
gke 0:62a1c91a859a 1254
gke 1:1e3318a30ddd 1255 extern boolean I2C0AddressResponds(uint8);
gke 1:1e3318a30ddd 1256 #ifdef HAVE_I2C1
gke 1:1e3318a30ddd 1257 extern boolean I2C1AddressResponds(uint8);
gke 1:1e3318a30ddd 1258 #endif // HAVE_I2C1
gke 0:62a1c91a859a 1259 extern void TrackMinI2CRate(uint32);
gke 0:62a1c91a859a 1260 extern void ShowI2CDeviceName(uint8);
gke 0:62a1c91a859a 1261 extern uint8 ScanI2CBus(void);
gke 0:62a1c91a859a 1262 extern boolean ESCWaitClkHi(void);
gke 0:62a1c91a859a 1263 extern void ProgramSlaveAddress(uint8);
gke 0:62a1c91a859a 1264 extern void ConfigureESCs(void);
gke 0:62a1c91a859a 1265
gke 0:62a1c91a859a 1266 extern uint32 MinI2CRate;
gke 0:62a1c91a859a 1267
gke 0:62a1c91a859a 1268 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1269
gke 0:62a1c91a859a 1270 // leds.c
gke 0:62a1c91a859a 1271
gke 0:62a1c91a859a 1272 #define PCA9551_ID 0xc0
gke 0:62a1c91a859a 1273
gke 0:62a1c91a859a 1274 #define DRV0M 0x0001
gke 0:62a1c91a859a 1275 #define DRV1M 0x0002
gke 0:62a1c91a859a 1276 #define DRV2M 0x0004
gke 0:62a1c91a859a 1277 #define DRV3M 0x0008
gke 0:62a1c91a859a 1278
gke 0:62a1c91a859a 1279 #define AUX0M 0x0010
gke 0:62a1c91a859a 1280 #define AUX1M 0x0020
gke 0:62a1c91a859a 1281 #define AUX2M 0x0040
gke 0:62a1c91a859a 1282 #define AUX3M 0x0080
gke 0:62a1c91a859a 1283
gke 0:62a1c91a859a 1284 #define BeeperM DRV0M
gke 0:62a1c91a859a 1285
gke 0:62a1c91a859a 1286 #define YellowM 0x0100
gke 0:62a1c91a859a 1287 #define RedM 0x0200
gke 0:62a1c91a859a 1288 #define GreenM 0x0400
gke 0:62a1c91a859a 1289 #define BlueM 0x0800
gke 0:62a1c91a859a 1290
gke 0:62a1c91a859a 1291 #define ALL_LEDS_ON LEDsOn(BlueM|RedM|GreenM|YellowM)
gke 0:62a1c91a859a 1292 #define ALL_LEDS_OFF LEDsOff(BlueM|RedM|GreenM|YellowM)
gke 0:62a1c91a859a 1293
gke 0:62a1c91a859a 1294 #define AUX_LEDS_ON LEDsOn(AUX0M|AUX1M|AUX2M|AUX3M)
gke 0:62a1c91a859a 1295 #define AUX_LEDS_OFF LEDsOff(AUX0M|AUX1M|AUX2M|AUX3M)
gke 0:62a1c91a859a 1296
gke 0:62a1c91a859a 1297 #define AUX_DRVS_ON LEDsOn(DRV0M|DRV1M|DRV2M|DRV3M)
gke 0:62a1c91a859a 1298 #define AUX_DRVS_OFF LEDsOff(DRV0M|DRV1M|DRV2M|DRV3M)
gke 0:62a1c91a859a 1299
gke 0:62a1c91a859a 1300 #define ALL_LEDS_ARE_OFF ( (LEDShadow&(BlueM|RedM|GreenM|YellowM))== (uint8)0 )
gke 0:62a1c91a859a 1301
gke 0:62a1c91a859a 1302 #define BEEPER_IS_ON ( (LEDShadow & BeeperM) != (uint8)0 )
gke 0:62a1c91a859a 1303 #define BEEPER_IS_OFF ( (LEDShadow & BeeperM) == (uint8)0 )
gke 0:62a1c91a859a 1304
gke 0:62a1c91a859a 1305 #define LEDRed_ON LEDsOn(RedM)
gke 0:62a1c91a859a 1306 #define LEDBlue_ON LEDsOn(BlueM)
gke 0:62a1c91a859a 1307 #define LEDGreen_ON LEDsOn(GreenM)
gke 0:62a1c91a859a 1308 #define LEDYellow_ON LEDsOn(YellowM)
gke 0:62a1c91a859a 1309 #define LEDAUX1_ON LEDsOn(AUX1M)
gke 0:62a1c91a859a 1310 #define LEDAUX2_ON LEDsOn(AUX2M)
gke 0:62a1c91a859a 1311 #define LEDAUX3_ON LEDsOn(AUX3M)
gke 0:62a1c91a859a 1312 #define LEDRed_OFF LEDsOff(RedM)
gke 0:62a1c91a859a 1313 #define LEDBlue_OFF LEDsOff(BlueM)
gke 0:62a1c91a859a 1314 #define LEDGreen_OFF LEDsOff(GreenM)
gke 0:62a1c91a859a 1315 #define LEDYellow_OFF LEDsOff(YellowM)
gke 0:62a1c91a859a 1316 #define LEDYellow_TOG if( (LEDShadow&YellowM) == (uint8)0 ) LEDsOn(YellowM); else LEDsOff(YellowM)
gke 0:62a1c91a859a 1317 #define LEDRed_TOG if( (LEDShadow&RedM) == (uint8)0 ) LEDsOn(RedM); else LEDsOff(RedM)
gke 0:62a1c91a859a 1318 #define LEDBlue_TOG if( (LEDShadow&BlueM) == (uint8)0 ) LEDsOn(BlueM); else LEDsOff(BlueM)
gke 0:62a1c91a859a 1319 #define LEDGreen_TOG if( (LEDShadow&GreenM) == (uint8)0 ) LEDsOn(GreenM); else LEDsOff(GreenM)
gke 0:62a1c91a859a 1320 #define Beeper_OFF LEDsOff(BeeperM)
gke 0:62a1c91a859a 1321 #define Beeper_ON LEDsOn(BeeperM)
gke 0:62a1c91a859a 1322 #define Beeper_TOG if( (LEDShadow&BeeperM) == (uint8)0 ) LEDsOn(BeeperM); else LEDsOff(BeeperM)
gke 0:62a1c91a859a 1323
gke 0:62a1c91a859a 1324 extern void SendLEDs(void);
gke 0:62a1c91a859a 1325 extern void SaveLEDs(void);
gke 0:62a1c91a859a 1326 extern void RestoreLEDs(void);
gke 0:62a1c91a859a 1327 extern void LEDsOn(uint16);
gke 0:62a1c91a859a 1328 extern void LEDsOff(uint16);
gke 0:62a1c91a859a 1329 extern void LEDChaser(void);
gke 0:62a1c91a859a 1330 extern void DoLEDs(void);
gke 0:62a1c91a859a 1331 extern void PowerOutput(int8);
gke 0:62a1c91a859a 1332 extern void LEDsAndBuzzer(void);
gke 0:62a1c91a859a 1333
gke 0:62a1c91a859a 1334 extern void PCA9551Test(void);
gke 0:62a1c91a859a 1335 extern void WritePCA9551(uint8);
gke 0:62a1c91a859a 1336 extern boolean IsPCA9551Active(void);
gke 0:62a1c91a859a 1337
gke 0:62a1c91a859a 1338 extern void InitLEDs(void);
gke 0:62a1c91a859a 1339
gke 0:62a1c91a859a 1340 extern uint16 LEDShadow, PrevLEDShadow, SavedLEDs, LEDPattern;
gke 0:62a1c91a859a 1341 extern uint8 PrevPCA9551LEDShadow;
gke 0:62a1c91a859a 1342
gke 0:62a1c91a859a 1343 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1344
gke 0:62a1c91a859a 1345 // math.c
gke 0:62a1c91a859a 1346
gke 0:62a1c91a859a 1347 extern int16 SRS16(int16, uint8);
gke 0:62a1c91a859a 1348 extern int32 SRS32(int32, uint8);
gke 0:62a1c91a859a 1349 extern real32 Make2Pi(real32);
gke 0:62a1c91a859a 1350 extern real32 MakePi(real32);
gke 0:62a1c91a859a 1351 extern int16 Table16(int16, const int16 *);
gke 0:62a1c91a859a 1352
gke 0:62a1c91a859a 1353 extern real32 VDot(real32 v1[3], real32 v2[3]);
gke 0:62a1c91a859a 1354 extern void VCross(real32 VOut[3], real32 v1[3], real32 v2[3]);
gke 0:62a1c91a859a 1355 extern void VScale(real32 VOut[3], real32 v[3], real32 s);
gke 0:62a1c91a859a 1356 extern void VAdd(real32 VOut[3],real32 v1[3], real32 v2[3]);
gke 0:62a1c91a859a 1357 extern void VSub(real32 VOut[3],real32 v1[3], real32 v2[3]);
gke 0:62a1c91a859a 1358
gke 0:62a1c91a859a 1359 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1360
gke 0:62a1c91a859a 1361 // menu.c
gke 0:62a1c91a859a 1362
gke 0:62a1c91a859a 1363 extern void ShowPrompt(void);
gke 0:62a1c91a859a 1364 extern void ShowRxSetup(void);
gke 0:62a1c91a859a 1365 extern void ShowSetup(boolean);
gke 0:62a1c91a859a 1366 extern uint8 MakeUpper(uint8);
gke 0:62a1c91a859a 1367 extern void ProcessCommand(void);
gke 0:62a1c91a859a 1368
gke 0:62a1c91a859a 1369 extern const uint8 SerHello[];
gke 0:62a1c91a859a 1370 extern const uint8 SerSetup[];
gke 0:62a1c91a859a 1371 extern const uint8 SerPrompt[];
gke 0:62a1c91a859a 1372
gke 0:62a1c91a859a 1373 extern const uint8 RxChMnem[];
gke 0:62a1c91a859a 1374
gke 0:62a1c91a859a 1375 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1376
gke 0:62a1c91a859a 1377 // nonvolatiles.c
gke 0:62a1c91a859a 1378
gke 0:62a1c91a859a 1379 extern void CheckSDCardValid(void);
gke 0:62a1c91a859a 1380
gke 0:62a1c91a859a 1381 extern void CreateLogfile(void);
gke 0:62a1c91a859a 1382 extern void CloseLogfile(void);
gke 0:62a1c91a859a 1383 extern void TxLogChar(uint8);
gke 0:62a1c91a859a 1384
gke 0:62a1c91a859a 1385 extern void WritePXImagefile(void);
gke 0:62a1c91a859a 1386 extern boolean ReadPXImagefile(void);
gke 0:62a1c91a859a 1387 extern int8 ReadPX(uint16);
gke 0:62a1c91a859a 1388 extern int16 Read16PX(uint16);
gke 0:62a1c91a859a 1389 extern int32 Read32PX(uint16);
gke 0:62a1c91a859a 1390 extern void WritePX(uint16, int8);
gke 0:62a1c91a859a 1391 extern void Write16PX(uint16, int16);
gke 0:62a1c91a859a 1392 extern void Write32PX(uint16, int32);
gke 0:62a1c91a859a 1393
gke 0:62a1c91a859a 1394 extern FILE *pxfile;
gke 0:62a1c91a859a 1395 extern FILE *newpxfile;
gke 0:62a1c91a859a 1396
gke 0:62a1c91a859a 1397 extern const int PX_LENGTH;
gke 0:62a1c91a859a 1398 extern int8 PX[], PXNew[];
gke 0:62a1c91a859a 1399
gke 0:62a1c91a859a 1400 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1401
gke 0:62a1c91a859a 1402 // outputs.c
gke 0:62a1c91a859a 1403
gke 0:62a1c91a859a 1404 #define OUT_MINIMUM 1.0 // Required for PPM timing loops
gke 0:62a1c91a859a 1405 #define OUT_MAXIMUM 200.0 // to reduce Rx capture and servo pulse output interaction
gke 0:62a1c91a859a 1406 #define OUT_NEUTRAL 105.0 // 1.503mS @ 105 16MHz
gke 0:62a1c91a859a 1407 #define OUT_HOLGER_MAXIMUM 225.0
gke 0:62a1c91a859a 1408 #define OUT_YGEI2C_MAXIMUM 240.0
gke 0:62a1c91a859a 1409 #define OUT_X3D_MAXIMUM 200.0
gke 0:62a1c91a859a 1410
gke 0:62a1c91a859a 1411 extern uint8 SaturInt(int16);
gke 0:62a1c91a859a 1412 extern void DoMulticopterMix(real32 CurrThrottle);
gke 0:62a1c91a859a 1413 extern void CheckDemand(real32 CurrThrottle);
gke 0:62a1c91a859a 1414 extern void MixAndLimitMotors(void);
gke 0:62a1c91a859a 1415 extern void MixAndLimitCam(void);
gke 0:62a1c91a859a 1416 extern void OutSignals(void);
gke 0:62a1c91a859a 1417 extern void InitI2CESCs(void);
gke 0:62a1c91a859a 1418 extern void StopMotors(void);
gke 0:62a1c91a859a 1419 extern void ExercisePWM(void);
gke 0:62a1c91a859a 1420 extern void InitMotors(void);
gke 0:62a1c91a859a 1421
gke 1:1e3318a30ddd 1422 // Using clockwise numbering - NOT the same as UAVXPIC
gke 0:62a1c91a859a 1423 enum PWMCamTags { CamRollC = 6, CamPitchC = 7 };
gke 1:1e3318a30ddd 1424 enum PWMQuadTags {FrontC=0, RightC, BackC, LeftC }; // order is important for X3D & Holger ESCs
gke 0:62a1c91a859a 1425 enum PWMConvTags {ThrottleC=0, AileronC, ElevatorC, RudderC, RTHC };
gke 0:62a1c91a859a 1426 enum PWMWingTags3 {RightElevonC=1, LeftElevonC=2};
gke 1:1e3318a30ddd 1427 enum PWMVTTags {FrontRightC=0, FrontLeftC};
gke 1:1e3318a30ddd 1428 enum PWMY6Tags {FrontTC=0, RightTC, LeftTC, FrontBC, RightBC, LeftBC };
gke 1:1e3318a30ddd 1429 enum PWMHexTags {FrontHC=0, FrontRightHC, BackRightHC, BackHC, BackLeftHC,FrontLeftHC };
gke 0:62a1c91a859a 1430
gke 0:62a1c91a859a 1431 //#define NoOfPWMOutputs 4
gke 1:1e3318a30ddd 1432 #ifdef HEXACOPTER
gke 1:1e3318a30ddd 1433 #define NoOfI2CESCOutputs 6
gke 1:1e3318a30ddd 1434 #else
gke 0:62a1c91a859a 1435 #define NoOfI2CESCOutputs 4
gke 1:1e3318a30ddd 1436 #endif // HEXACOPTER
gke 0:62a1c91a859a 1437
gke 0:62a1c91a859a 1438 extern const real32 PWMScale;
gke 0:62a1c91a859a 1439
gke 0:62a1c91a859a 1440 extern real32 PWM[8];
gke 0:62a1c91a859a 1441 extern real32 PWMSense[8];
gke 0:62a1c91a859a 1442 extern int16 ESCI2CFail[6];
gke 0:62a1c91a859a 1443 extern int16 CurrThrottle;
gke 0:62a1c91a859a 1444 extern int16 CamRollPulseWidth, CamPitchPulseWidth;
gke 0:62a1c91a859a 1445 extern int16 ESCMin, ESCMax;
gke 0:62a1c91a859a 1446
gke 0:62a1c91a859a 1447 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1448
gke 0:62a1c91a859a 1449 // params.c
gke 0:62a1c91a859a 1450
gke 0:62a1c91a859a 1451 extern void ReadParameters(void);
gke 0:62a1c91a859a 1452 extern void UseDefaultParameters(void);
gke 0:62a1c91a859a 1453 extern void UpdateParamSetChoice(void);
gke 0:62a1c91a859a 1454 extern boolean ParameterSanityCheck(void);
gke 0:62a1c91a859a 1455 extern void InitParameters(void);
gke 0:62a1c91a859a 1456
gke 0:62a1c91a859a 1457 enum TxRxTypes {
gke 0:62a1c91a859a 1458 FutabaCh3, FutabaCh2, FutabaDM8, JRPPM, JRDM9, JRDXS12,
gke 0:62a1c91a859a 1459 DX7AR7000, DX7AR6200, FutabaCh3_6_7, DX7AR6000, GraupnerMX16s, DX6iAR6200, FutabaCh3_R617FS,
gke 0:62a1c91a859a 1460 DX7aAR7000, ExternalDecoder, FrSkyDJT_D8R, UnknownTxRx, CustomTxRx
gke 0:62a1c91a859a 1461 };
gke 0:62a1c91a859a 1462 enum RCControls {ThrottleRC, RollRC, PitchRC, YawRC, RTHRC, CamPitchRC, NavGainRC};
gke 0:62a1c91a859a 1463 enum ESCTypes { ESCPPM, ESCHolger, ESCX3D, ESCYGEI2C };
gke 0:62a1c91a859a 1464 enum AFs { QuadAF, TriAF, VTAF, Y6AF, HeliAF, ElevAF, AilAF };
gke 0:62a1c91a859a 1465
gke 0:62a1c91a859a 1466 enum Params { // MAX 64
gke 0:62a1c91a859a 1467 RollKp, // 01
gke 0:62a1c91a859a 1468 RollKi, // 02
gke 0:62a1c91a859a 1469 RollKd, // 03
gke 0:62a1c91a859a 1470 HorizDampKp, // 04c
gke 0:62a1c91a859a 1471 RollIntLimit, // 05
gke 0:62a1c91a859a 1472 PitchKp, // 06
gke 0:62a1c91a859a 1473 PitchKi, // 07
gke 0:62a1c91a859a 1474 PitchKd, // 08
gke 0:62a1c91a859a 1475 AltKp, // 09c
gke 0:62a1c91a859a 1476 PitchIntLimit, // 10
gke 0:62a1c91a859a 1477
gke 0:62a1c91a859a 1478 YawKp, // 11
gke 0:62a1c91a859a 1479 YawKi, // 12
gke 0:62a1c91a859a 1480 YawKd, // 13
gke 0:62a1c91a859a 1481 YawLimit, // 14
gke 0:62a1c91a859a 1482 YawIntLimit, // 15
gke 0:62a1c91a859a 1483 ConfigBits, // 16c
gke 0:62a1c91a859a 1484 unused17 , // 17 TimeSlots
gke 0:62a1c91a859a 1485 LowVoltThres, // 18c
gke 0:62a1c91a859a 1486 CamRollKp, // 19
gke 0:62a1c91a859a 1487 PercentCruiseThr, // 20c
gke 0:62a1c91a859a 1488
gke 0:62a1c91a859a 1489 VertDampKp, // 21c
gke 0:62a1c91a859a 1490 MiddleUD, // 22c
gke 0:62a1c91a859a 1491 PercentIdleThr, // 23c
gke 0:62a1c91a859a 1492 MiddleLR, // 24c
gke 0:62a1c91a859a 1493 MiddleBF, // 25c
gke 0:62a1c91a859a 1494 CamPitchKp, // 26
gke 0:62a1c91a859a 1495 CompassKp, // 27
gke 0:62a1c91a859a 1496 AltKi, // 28c
gke 0:62a1c91a859a 1497 unused29 , // 29 NavRadius
gke 0:62a1c91a859a 1498 NavKi, // 30
gke 0:62a1c91a859a 1499
gke 0:62a1c91a859a 1500 GSThrottle, // 31
gke 0:62a1c91a859a 1501 Acro, // 32
gke 0:62a1c91a859a 1502 NavRTHAlt, // 33
gke 0:62a1c91a859a 1503 NavMagVar, // 34c
gke 0:62a1c91a859a 1504 GyroRollPitchType, // 35
gke 0:62a1c91a859a 1505 ESCType, // 36
gke 0:62a1c91a859a 1506 TxRxType, // 37
gke 0:62a1c91a859a 1507 NeutralRadius, // 38
gke 0:62a1c91a859a 1508 PercentNavSens6Ch, // 39
gke 0:62a1c91a859a 1509 CamRollTrim, // 40
gke 0:62a1c91a859a 1510 NavKd, // 41
gke 0:62a1c91a859a 1511 VertDampDecay, // 42
gke 0:62a1c91a859a 1512 HorizDampDecay, // 43
gke 0:62a1c91a859a 1513 BaroScale, // 44
gke 0:62a1c91a859a 1514 TelemetryType, // 45
gke 0:62a1c91a859a 1515 MaxDescentRateDmpS, // 46
gke 0:62a1c91a859a 1516 DescentDelayS, // 47
gke 0:62a1c91a859a 1517 NavIntLimit, // 48
gke 0:62a1c91a859a 1518 AltIntLimit, // 49
gke 0:62a1c91a859a 1519 unused50, // 50 GravComp
gke 0:62a1c91a859a 1520 unused51 , // 51 CompSteps
gke 0:62a1c91a859a 1521 ServoSense, // 52
gke 0:62a1c91a859a 1522 CompassOffsetQtr, // 53
gke 0:62a1c91a859a 1523 BatteryCapacity, // 54
gke 0:62a1c91a859a 1524 unused55, // 55 GyroYawType
gke 0:62a1c91a859a 1525 AltKd, // 56
gke 0:62a1c91a859a 1526 Orient, // 57
gke 0:62a1c91a859a 1527 NavYawLimit, // 58
gke 0:62a1c91a859a 1528 Balance // 59
gke 0:62a1c91a859a 1529
gke 0:62a1c91a859a 1530 // 56 - 64 unused currently
gke 0:62a1c91a859a 1531 };
gke 0:62a1c91a859a 1532
gke 0:62a1c91a859a 1533 //#define FlyXMode 0
gke 0:62a1c91a859a 1534 //#define FlyAltOrientationMask 0x01
gke 0:62a1c91a859a 1535
gke 0:62a1c91a859a 1536 #define UsePositionHoldLock 0
gke 0:62a1c91a859a 1537 #define UsePositionHoldLockMask 0x01
gke 0:62a1c91a859a 1538
gke 0:62a1c91a859a 1539 #define UseRTHDescend 1
gke 0:62a1c91a859a 1540 #define UseRTHDescendMask 0x02
gke 0:62a1c91a859a 1541
gke 0:62a1c91a859a 1542 #define TxMode2 2
gke 0:62a1c91a859a 1543 #define TxMode2Mask 0x04
gke 0:62a1c91a859a 1544
gke 0:62a1c91a859a 1545 #define RxSerialPPM 3
gke 0:62a1c91a859a 1546 #define RxSerialPPMMask 0x08
gke 0:62a1c91a859a 1547
gke 0:62a1c91a859a 1548 #define RFInches 4
gke 0:62a1c91a859a 1549 #define RFInchesMask 0x10
gke 0:62a1c91a859a 1550
gke 0:62a1c91a859a 1551 // bit 4 is pulse polarity for 3.15
gke 0:62a1c91a859a 1552
gke 0:62a1c91a859a 1553 #define UseUseAngleControl 5
gke 0:62a1c91a859a 1554 #define UseAngleControlMask 0x20
gke 0:62a1c91a859a 1555
gke 0:62a1c91a859a 1556 #define UsePolar 6
gke 0:62a1c91a859a 1557 #define UsePolarMask 0x40
gke 0:62a1c91a859a 1558
gke 0:62a1c91a859a 1559 // bit 7 unusable in UAVPSet
gke 0:62a1c91a859a 1560
gke 0:62a1c91a859a 1561 extern const int8 DefaultParams[MAX_PARAMETERS][2];
gke 0:62a1c91a859a 1562
gke 0:62a1c91a859a 1563 extern const uint8 ESCLimits [];
gke 0:62a1c91a859a 1564
gke 0:62a1c91a859a 1565 extern real32 OSin[], OCos[];
gke 0:62a1c91a859a 1566 extern uint8 Orientation, PolarOrientation;
gke 0:62a1c91a859a 1567
gke 0:62a1c91a859a 1568 extern uint8 ParamSet;
gke 0:62a1c91a859a 1569 extern boolean ParametersChanged, SaveAllowTurnToWP;
gke 0:62a1c91a859a 1570 extern int8 P[];
gke 0:62a1c91a859a 1571 extern real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate
gke 0:62a1c91a859a 1572
gke 0:62a1c91a859a 1573 extern uint8 UAVXAirframe;
gke 0:62a1c91a859a 1574
gke 0:62a1c91a859a 1575 //__________________________________________________________________________________________
gke 0:62a1c91a859a 1576
gke 0:62a1c91a859a 1577 // rc.c
gke 0:62a1c91a859a 1578
gke 0:62a1c91a859a 1579 extern void DoRxPolarity(void);
gke 0:62a1c91a859a 1580 extern void InitRC(void);
gke 0:62a1c91a859a 1581 extern void MapRC(void);
gke 0:62a1c91a859a 1582 extern void CheckSticksHaveChanged(void);
gke 0:62a1c91a859a 1583 extern void UpdateControls(void);
gke 0:62a1c91a859a 1584 extern void CaptureTrims(void);
gke 0:62a1c91a859a 1585 extern void CheckThrottleMoved(void);
gke 0:62a1c91a859a 1586 extern void ReceiverTest(void);
gke 0:62a1c91a859a 1587
gke 0:62a1c91a859a 1588 extern const boolean PPMPosPolarity[];
gke 0:62a1c91a859a 1589 extern const uint8 Map[CustomTxRx+1][CONTROLS];
gke 0:62a1c91a859a 1590 extern int8 RMap[];
gke 0:62a1c91a859a 1591
gke 0:62a1c91a859a 1592 #define PPMQMASK 3
gke 0:62a1c91a859a 1593 extern int16 PPMQSum[];
gke 0:62a1c91a859a 1594 extern int16x8x4Q PPMQ;
gke 0:62a1c91a859a 1595 extern boolean RCPositiveEdge;
gke 0:62a1c91a859a 1596 extern int16 RC[], RCp[];
gke 0:62a1c91a859a 1597 extern int16 Trim[3];
gke 0:62a1c91a859a 1598 extern int16 ThrLow, ThrNeutral, ThrHigh;
gke 0:62a1c91a859a 1599
gke 0:62a1c91a859a 1600 //__________________________________________________________________________________________
gke 0:62a1c91a859a 1601
gke 0:62a1c91a859a 1602 // serial.c
gke 0:62a1c91a859a 1603
gke 0:62a1c91a859a 1604 extern void TxString(const uint8*);
gke 0:62a1c91a859a 1605 extern void TxChar(uint8);
gke 0:62a1c91a859a 1606 extern void TxValU(uint8);
gke 0:62a1c91a859a 1607 extern void TxValS(int8);
gke 0:62a1c91a859a 1608 extern void TxBin8(uint8);
gke 0:62a1c91a859a 1609 extern void TxNextLine(void);
gke 0:62a1c91a859a 1610 extern void TxNibble(uint8);
gke 0:62a1c91a859a 1611 extern void TxValH( uint8);
gke 0:62a1c91a859a 1612 extern void TxValH16(uint16);
gke 0:62a1c91a859a 1613 extern uint8 RxChar(void);
gke 0:62a1c91a859a 1614 extern uint8 PollRxChar(void);
gke 0:62a1c91a859a 1615 extern uint8 RxNumU(void);
gke 0:62a1c91a859a 1616 extern int8 RxNumS(void);
gke 0:62a1c91a859a 1617 extern void TxVal32(int32, int8, uint8);
gke 0:62a1c91a859a 1618 extern void TxChar(uint8);
gke 0:62a1c91a859a 1619 extern void TxESCu8(uint8);
gke 0:62a1c91a859a 1620 extern void Sendi16(int16);
gke 0:62a1c91a859a 1621 extern void TxESCi8(int8);
gke 0:62a1c91a859a 1622 extern void TxESCi16(int16);
gke 0:62a1c91a859a 1623 extern void TxESCi24(int24);
gke 0:62a1c91a859a 1624 extern void TxESCi32(int32);
gke 0:62a1c91a859a 1625
gke 0:62a1c91a859a 1626 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1627
gke 0:62a1c91a859a 1628 // stats.c
gke 0:62a1c91a859a 1629
gke 0:62a1c91a859a 1630 extern void ZeroStats(void);
gke 0:62a1c91a859a 1631 extern void ReadStatsPX(void);
gke 0:62a1c91a859a 1632 extern void WriteStatsPX(void);
gke 0:62a1c91a859a 1633 extern void ShowStats(void);
gke 0:62a1c91a859a 1634
gke 0:62a1c91a859a 1635 enum Statistics {
gke 0:62a1c91a859a 1636 GPSAltitudeS, BaroRelAltitudeS, ESCI2CFailS, GPSMinSatsS, MinROCS, MaxROCS, GPSVelS,
gke 0:62a1c91a859a 1637 AccFailS, CompassFailS, BaroFailS, GPSInvalidS, GPSMaxSatsS, NavValidS,
gke 0:62a1c91a859a 1638 MinHDiluteS, MaxHDiluteS, RCGlitchesS, GPSBaroScaleS, GyroFailS, RCFailsafesS, I2CFailS, MinTempS, MaxTempS, BadS, BadNumS
gke 0:62a1c91a859a 1639 }; // NO MORE THAN 32 or 64 bytes
gke 0:62a1c91a859a 1640
gke 0:62a1c91a859a 1641 extern int16 Stats[];
gke 0:62a1c91a859a 1642
gke 0:62a1c91a859a 1643 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1644
gke 0:62a1c91a859a 1645 // telemetry.c
gke 0:62a1c91a859a 1646
gke 0:62a1c91a859a 1647 extern void RxTelemetryPacket(uint8);
gke 0:62a1c91a859a 1648 extern void InitTelemetryPacket(void);
gke 0:62a1c91a859a 1649 extern void BuildTelemetryPacket(uint8);
gke 0:62a1c91a859a 1650
gke 0:62a1c91a859a 1651 extern void SendPacketHeader(void);
gke 0:62a1c91a859a 1652 extern void SendPacketTrailer(void);
gke 0:62a1c91a859a 1653
gke 0:62a1c91a859a 1654 extern void SendTelemetry(void);
gke 0:62a1c91a859a 1655 extern void SendUAVX(void);
gke 0:62a1c91a859a 1656 extern void SendUAVXControl(void);
gke 0:62a1c91a859a 1657 extern void SendFlightPacket(void);
gke 0:62a1c91a859a 1658 extern void SendNavPacket(void);
gke 0:62a1c91a859a 1659 extern void SendControlPacket(void);
gke 0:62a1c91a859a 1660 extern void SendStatsPacket(void);
gke 1:1e3318a30ddd 1661 extern void SendParamPacket(uint8, uint8);
gke 1:1e3318a30ddd 1662 extern void SendParameters(uint8);
gke 0:62a1c91a859a 1663 extern void SendMinPacket(void);
gke 0:62a1c91a859a 1664 extern void SendArduStation(void);
gke 0:62a1c91a859a 1665 extern void SendCustom(void);
gke 0:62a1c91a859a 1666 extern void SensorTrace(void);
gke 0:62a1c91a859a 1667 extern void CheckTelemetry(void);
gke 0:62a1c91a859a 1668
gke 0:62a1c91a859a 1669 enum TelemetryStates { WaitRxSentinel, WaitRxESC, WaitRxBody };
gke 0:62a1c91a859a 1670
gke 0:62a1c91a859a 1671
gke 0:62a1c91a859a 1672 enum PacketTags {UnknownPacketTag = 0, LevPacketTag, NavPacketTag, MicropilotPacketTag, WayPacketTag,
gke 0:62a1c91a859a 1673 AirframePacketTag, NavUpdatePacketTag, BasicPacketTag, RestartPacketTag, TrimblePacketTag,
gke 0:62a1c91a859a 1674 MessagePacketTag, EnvironmentPacketTag, BeaconPacketTag, UAVXFlightPacketTag,
gke 1:1e3318a30ddd 1675 UAVXNavPacketTag, UAVXStatsPacketTag, UAVXControlPacketTag, UAVXParamPacketTag, UAVXMinPacketTag,
gke 1:1e3318a30ddd 1676 UAVXArmParamPacketTag, UAVXStickPacketTag, FrSkyPacketTag = 99
gke 0:62a1c91a859a 1677 };
gke 0:62a1c91a859a 1678
gke 0:62a1c91a859a 1679 enum TelemetryTypes { NoTelemetry, GPSTelemetry, UAVXTelemetry, UAVXControlTelemetry, UAVXMinTelemetry,
gke 0:62a1c91a859a 1680 ArduStationTelemetry, CustomTelemetry
gke 0:62a1c91a859a 1681 };
gke 0:62a1c91a859a 1682
gke 0:62a1c91a859a 1683 extern uint8 UAVXCurrPacketTag;
gke 0:62a1c91a859a 1684 extern uint8 RxPacketLength, RxPacketByteCount;
gke 0:62a1c91a859a 1685 extern uint8 RxCheckSum;
gke 0:62a1c91a859a 1686 extern uint8 RxPacketTag, ReceivedPacketTag;
gke 0:62a1c91a859a 1687 extern uint8 PacketRxState;
gke 0:62a1c91a859a 1688 extern boolean CheckSumError, TelemetryPacketReceived;
gke 0:62a1c91a859a 1689
gke 0:62a1c91a859a 1690 extern int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
gke 0:62a1c91a859a 1691
gke 0:62a1c91a859a 1692 extern uint8 TxCheckSum;
gke 0:62a1c91a859a 1693
gke 0:62a1c91a859a 1694 extern FILE *logfile;
gke 0:62a1c91a859a 1695 extern boolean EchoToLogFile, LogfileIsOpen;
gke 0:62a1c91a859a 1696 extern uint32 LogChars;
gke 0:62a1c91a859a 1697
gke 0:62a1c91a859a 1698 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1699
gke 0:62a1c91a859a 1700 // temperature.c
gke 0:62a1c91a859a 1701
gke 0:62a1c91a859a 1702 #define TMP100_MAX_ADC 4095 // 12 bits
gke 0:62a1c91a859a 1703 #define TMP100_ID 0x96
gke 0:62a1c91a859a 1704 #define TMP100_WR 0x96 // Write
gke 0:62a1c91a859a 1705 #define TMP100_RD 0x97 // Read
gke 0:62a1c91a859a 1706 #define TMP100_TMP 0x00 // Temperature
gke 0:62a1c91a859a 1707 #define TMP100_CMD 0x01
gke 0:62a1c91a859a 1708 #define TMP100_LOW 0x02 // Alarm low limit
gke 0:62a1c91a859a 1709 #define TMP100_HI 0x03 // Alarm high limit
gke 0:62a1c91a859a 1710 #define TMP100_CFG 0 // 0.5 deg resolution continuous
gke 0:62a1c91a859a 1711
gke 0:62a1c91a859a 1712 extern void GetTemperature(void);
gke 0:62a1c91a859a 1713 extern void InitTemperature(void);
gke 0:62a1c91a859a 1714
gke 0:62a1c91a859a 1715 extern i16u AmbientTemperature;
gke 0:62a1c91a859a 1716
gke 0:62a1c91a859a 1717 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1718
gke 0:62a1c91a859a 1719 // utils.c
gke 0:62a1c91a859a 1720
gke 0:62a1c91a859a 1721 extern void InitMisc(void);
gke 0:62a1c91a859a 1722 extern void Delay1mS(int16);
gke 0:62a1c91a859a 1723 extern void Delay100mS(int16);
gke 0:62a1c91a859a 1724 extern void DoBeep100mS(uint8, uint8);
gke 0:62a1c91a859a 1725 extern void DoStartingBeeps(uint8);
gke 0:62a1c91a859a 1726 extern real32 SlewLimit(real32, real32, real32);
gke 0:62a1c91a859a 1727 extern real32 DecayX(real32, real32);
gke 0:62a1c91a859a 1728 extern void LPFilter(real32*, real32*, real32, real32);
gke 0:62a1c91a859a 1729 extern void CheckAlarms(void);
gke 0:62a1c91a859a 1730 extern void Timing(uint8, uint32);
gke 0:62a1c91a859a 1731
gke 0:62a1c91a859a 1732 typedef struct {
gke 0:62a1c91a859a 1733 uint32 T;
gke 0:62a1c91a859a 1734 uint32 Count;
gke 0:62a1c91a859a 1735 } TimingRec;
gke 0:62a1c91a859a 1736
gke 0:62a1c91a859a 1737 enum Timed { GetAttitudeT , UnknownT };
gke 0:62a1c91a859a 1738 extern TimingRec Times[];
gke 0:62a1c91a859a 1739
gke 0:62a1c91a859a 1740 #define TimeS uint32 TStart;TStart=timer.read_us();
gke 0:62a1c91a859a 1741 #define TimeF(w, tf) Time(w, tf)
gke 0:62a1c91a859a 1742
gke 0:62a1c91a859a 1743 //______________________________________________________________________________________________
gke 0:62a1c91a859a 1744
gke 0:62a1c91a859a 1745
gke 0:62a1c91a859a 1746 // Sanity checks
gke 0:62a1c91a859a 1747
gke 0:62a1c91a859a 1748 #if (( defined TRICOPTER + defined QUADROCOPTER + defined VTCOPTER + defined Y6COPTER + defined HELICOPTER + defined AILERON + defined ELEVON ) != 1)
gke 0:62a1c91a859a 1749 #error None or more than one aircraft configuration defined !
gke 0:62a1c91a859a 1750 #endif