Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
  • 2 commits
  • 4 files changed
  • 0 comments
  • 1 contributor

Showing 4 changed files with 57 additions and 56 deletions. Show diff stats Hide diff stats

  1. +39 44 src/globals.h
  2. +16 10 src/imu.cpp
  3. +1 1  src/motors.h
  4. +1 1  src/pilot.cpp
83 src/globals.h
@@ -10,19 +10,15 @@
10 10
11 11 // #define DEBUG
12 12
13   -/*****************************************************************************
14   - * Variables
15   - *****************************************************************************/
16   -
  13 +// ============================================================================
  14 +// Variables
  15 +// ============================================================================
17 16 int armCount; // Arm status counter.
18 17 int loopCount; // Count system loops.
19   -uint16_t pwmOut[4];
20   -char commStr[250]; // String to be sent out to base.
  18 +uint16_t pwmOut[4]; // 10 bit PWM output duty cycle.
21 19 float bodyDCM[3][3]; // Current body orientation calculated by IMU.
22 20 float targetRot[3], currentRot[3], pidRot[3];
23 21
24   -float rateX, rateY;
25   -
26 22
27 23 // ============================================================================
28 24 // PID
@@ -35,12 +31,7 @@ struct PIDdata {
35 31 float lastValue;
36 32 float integral;
37 33 float lastDerivative;
38   -} PID[4];
39   -
40   -//#define PID_MOTOR_T 0
41   -//#define PID_MOTOR_R 1
42   -//#define PID_MOTOR_L 2
43   -//#define PID_SERVO_T 3
  34 +} PID[3];
44 35
45 36 #define PID_ROT_X 0
46 37 #define PID_ROT_Y 1
@@ -61,9 +52,6 @@ struct PIDdata {
61 52 #define THROTTLE_LOCK_DIFF_UP 70 // "Lock" throttle input. See pilot.h.
62 53 #define THROTTLE_LOCK_DIFF_DOWN 120 // "Lock" throttle input. See pilot.h.
63 54
64   -/*****************************************************************************
65   - * Serial: everything that has to do with TX/RX.
66   - *****************************************************************************/
67 55
68 56 // ============================================================================
69 57 // SERIAL IN
@@ -83,11 +71,11 @@ struct PIDdata {
83 71 // ============================================================================
84 72 // SERIAL OUT
85 73 // ============================================================================
86   -#define SEND_ARM_STATUS
87   -//#define SEND_TARGET_ROTATION
88   -#define SEND_MOTOR_VALUES
89   -#define SEND_DCM
90   -#define SEND_PID_DATA
  74 +//#define SEND_ARM_STATUS
  75 +////#define SEND_TARGET_ROTATION
  76 +//#define SEND_MOTOR_VALUES
  77 +//#define SEND_DCM
  78 +//#define SEND_PID_DATA
91 79
92 80 #define DCM_SER_TAG 0xfb
93 81 #define ROT_SER_TAG 0xfc
@@ -95,20 +83,16 @@ struct PIDdata {
95 83 #define PID_SER_TAG 0xfe
96 84 #define FIELD_SER_TAG 0xff
97 85
98   -/*****************************************************************************
99   - * Software configuration: any parameter that is purely code-related or is
100   - * relatively frequently changed.
101   - *****************************************************************************/
102 86
  87 +// ============================================================================
  88 +// Software configuration: any parameter that is purely code-related or is
  89 +// relatively frequently changed.
  90 +// ============================================================================
103 91 #define MASTER_DT 8000 // 8000 us interval = 125 Hz master loop.
104 92 #define CONTROL_LOOP_INTERVAL 1 // 1x master = 125 Hz.
105 93 #define COMM_LOOP_INTERVAL 5 // 1/5 master = 25 Hz. This frequency should be HIGHER than groundstation.py's dataSend frequency!
106 94 #define DOGLIFE 600 // Watchdog life in milliseconds.
107 95
108   -//#define DCM_COEFF 90 // Scale current-to-target DCM difference.
109   -//#define GYRO_COEFF 15 // Try to stabilize craft.
110   -//#define ACCEL_COEFF 90 // TEST: Try to stabilize craft.
111   -
112 96 // Throttle stuff. Minimum signal is 750 ms. Maximum signal is 2200 ms. Hover
113 97 // is around 1200 ms.
114 98 #define TMIN 450 // Minimum throttle PWM duty cycle (450/1023, or 1100 ms at 400 kHz)
@@ -127,6 +111,7 @@ struct PIDdata {
127 111 #define MOTOR_L 2 // Left motor array index.
128 112 #define SERVO_T 3 // Tail servo array index.
129 113
  114 +
130 115 // ============================================================================
131 116 // Buttons
132 117 // ============================================================================
@@ -143,10 +128,10 @@ struct PIDdata {
143 128 #define BUTTON_DECREASE_XY_D_GAIN 10
144 129 #define BUTTON_INCREASE_XY_D_GAIN 11
145 130
146   -/*****************************************************************************
147   - * Hardware configuration: any parameter that is changed so infrequently that
148   - * it may as well be hard-coded.
149   - *****************************************************************************/
  131 +// ============================================================================
  132 +// Hardware configuration: any parameter that is changed so infrequently that
  133 +// it may as well be hard-coded.
  134 +// ============================================================================
150 135
151 136 #define MOTOR_T_OFFSET 0 // Speed offset for tail motor.
152 137 #define MOTOR_R_OFFSET 0 // Speed offset for right motor.
@@ -157,20 +142,30 @@ struct PIDdata {
157 142 #define TAIL_SERVO_SCALE 1 // Scale tail servo rotation.
158 143 #define Z_ROT_SPEED 1 // Scale how much joystick twist input affects target Z rotation. A value of 1 here means a maximum Z rotation speed is 1 rad/s.
159 144
160   -// "Offset" values for accelerometer.
  145 +// Calibration values for accelerometer.
161 146 #define ACCEL_X_OFFSET -0.033
162 147 #define ACCEL_Y_OFFSET -0.011
163 148 #define ACCEL_Z_OFFSET -0.999
164 149
  150 +// Calibration values for magnetometer. These are what the magnetometer axes
  151 +// see as "zero". TODO: This is not entirely accurate. Need to figure out how
  152 +// magnetometers actually work!
  153 +#define MAG_X_MIN -314
  154 +#define MAG_X_MAX 320
  155 +#define MAG_Y_MIN -316
  156 +#define MAG_Y_MAX 317
  157 +#define MAG_Z_MIN -427
  158 +#define MAG_Z_MAX 165
  159 +
165 160 #define PMT 5 // Tail motor pin.
166 161 #define PMR 2 // Right motor pin.
167 162 #define PML 3 // Left motor pin.
168 163 #define PST 4 // Tail servo pin.
169 164
170 165
171   -/*****************************************************************************
172   - * Flight modes: not yet implemented.
173   - *****************************************************************************/
  166 +// ============================================================================
  167 +// Flight modes: not yet implemented.
  168 +// ============================================================================
174 169
175 170 //#define OFF 0
176 171 //#define IDLE 1
@@ -180,16 +175,16 @@ struct PIDdata {
180 175 //#define AUTO_HOVER 5
181 176
182 177
183   -/*****************************************************************************
184   - * Constants
185   - *****************************************************************************/
  178 +// ============================================================================
  179 +// Constants
  180 +// ============================================================================
186 181
187 182 #define PI 3.141592653589793238462643383279502884197f
188 183
189 184
190   -/*****************************************************************************
191   - * Functions
192   - *****************************************************************************/
  185 +// ============================================================================
  186 +// Functions
  187 +// ============================================================================
193 188
194 189 void zeroStr(char *sStr) {
195 190 for (int i=0; i<sizeof(sStr); i++) {
26 src/imu.cpp
@@ -128,13 +128,13 @@ void IMU::update() {
128 128
129 129 // Uncomment the loop below to get accelerometer readings in order to
130 130 // obtain wAOffset.
131   - //if (loopCount % COMM_LOOP_INTERVAL == 0) {
132   - // sp("(");
133   - // sp(aVec[0]*1000); sp(", ");
134   - // sp(aVec[1]*1000); sp(", ");
135   - // sp(aVec[2]*1000);
136   - // spln(")");
137   - //}
  131 + if (loopCount % COMM_LOOP_INTERVAL == 0) {
  132 + sp("A(");
  133 + sp(aVec[0]*1000); sp(", ");
  134 + sp(aVec[1]*1000); sp(", ");
  135 + sp(aVec[2]*1000);
  136 + sp(") ");
  137 + }
138 138
139 139 // Express K global unit vector in BODY frame as kgb for use in drift
140 140 // correction (we need K to be described in the BODY frame because gravity
@@ -167,6 +167,15 @@ void IMU::update() {
167 167 for (int i=0; i<3; i++) {
168 168 mVec[i] = mag.get(i);
169 169 }
  170 + if (loopCount % COMM_LOOP_INTERVAL == 0) {
  171 + sp("M(");
  172 + sp(mVec[0]); sp(", ");
  173 + sp(mVec[1]); sp(", ");
  174 + sp(mVec[2]);
  175 + spln(")");
  176 + }
  177 +
  178 +
170 179
171 180 // Express J global unit vectory in BODY frame as jgb.
172 181 for (int i=0; i<3; i++) {
@@ -189,9 +198,6 @@ void IMU::update() {
189 198 gVec[i] = gyro.get(i);
190 199 }
191 200
192   - rateX = gVec[0];
193   - rateY = gVec[1];
194   -
195 201 // Scale gVec by elapsed time (in seconds) to get angle w*dt in radians,
196 202 // then compute weighted average with the accelerometer and magnetometer
197 203 // correction vectors to obtain final w*dt.
2  src/motors.h
@@ -10,7 +10,7 @@
10 10
11 11 #include "globals.h"
12 12
13   -void calculate_pwm_output(float inputThrottle, float* inputRot) {
  13 +void calculate_pwm_outputs(float inputThrottle, float* inputRot) {
14 14 // ====================================================================
15 15 // Calculate motor/servo values.
16 16 // MOTOR_X_OFFSET: Offset starting motor values to account for
2  src/pilot.cpp
@@ -159,7 +159,7 @@ void Pilot::fly() {
159 159 throttleLock = throttle;
160 160 }
161 161
162   - calculate_pwm_output(throttle, pidRot);
  162 + calculate_pwm_outputs(throttle, pidRot);
163 163
164 164 okayToFly = false;
165 165 }

No commit comments for this range

Something went wrong with that request. Please try again.