Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

UAVXArm.h@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

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