Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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?
User | Revision | Line number | New 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 |