Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

UAVXArm.h@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

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