├── LICENSE ├── README.md ├── controller ├── GPS.h ├── PID.h ├── PilotCommandProcessor.h ├── SerialCommunication.h ├── controller.h ├── controller.ino ├── dataStorage.h ├── math.h └── sensors.h ├── extras ├── Configurator │ ├── background.js │ ├── css │ │ └── style.css │ ├── js │ │ ├── data_storage.js │ │ ├── file_system.js │ │ ├── flotr2.min.js │ │ ├── jquery-2.0.2.min.js │ │ ├── main.js │ │ ├── serial_backend.js │ │ └── union_process.js │ ├── main.html │ ├── manifest.json │ ├── readme.txt │ └── tabs │ │ ├── about.html │ │ ├── channel_assigner.html │ │ ├── channel_assigner.js │ │ ├── function_triggers.html │ │ ├── function_triggers.js │ │ ├── gps.html │ │ ├── gps.js │ │ ├── initial_setup.html │ │ ├── initial_setup.js │ │ ├── motor_output.html │ │ ├── motor_output.js │ │ ├── pid_tuning.html │ │ ├── pid_tuning.js │ │ ├── rx.html │ │ ├── rx.js │ │ ├── sensor_data.html │ │ ├── sensor_data.js │ │ ├── sensor_mapping.html │ │ ├── sensor_mapping.js │ │ ├── vehicle_view.html │ │ └── vehicle_view.js ├── Filters │ └── MAF.h ├── I2Cscanner │ └── I2Cscanner.ino └── PCB │ └── Phoenix_shield_v0.1.zip └── libraries ├── P_6DOF ├── mpu6050_10DOF_stick_px01.h └── mpu6050_6DOF_stick_px01.h ├── P_Accelerometer ├── ADXL345.h ├── ADXL345_AQ_v21.h └── BMA180_AQ_v20.h ├── P_Barometer ├── Barometer_bmp085.h └── Barometer_ms5611.h ├── P_BatteryMonitor ├── BatteryMonitor_current.h └── BatteryMonitor_voltage.h ├── P_ESC ├── ESC_328p_HW.h └── ESC_teensy3_HW.h ├── P_FrameType ├── FrameType_QuadPlus.h └── FrameType_QuadX.h ├── P_GPS ├── GPS_ublox.h └── neo-6m_configuration.txt ├── P_Gyroscope ├── ITG3200_AQ_v20.h └── ITG3200_AQ_v21.h ├── P_Kinematics ├── kinematics_ARG.h ├── kinematics_CMP.h └── kinematics_DCM.h ├── P_Magnetometer └── Magnetometer_HMC5883L.h ├── P_Receiver ├── Receiver_328p_HW_PPM.h ├── Receiver_teensy3_HW_PPM.h └── Receiver_teensy3_HW_PWM.h └── P_Sonar └── Sonar_srf04.h /README.md: -------------------------------------------------------------------------------- 1 | Flight Controller based on K20DX128 (Teensy 3.0) 2 | ------------------------------------------------ 3 | 4 | PIN setup (Teensy 3.0 pin numbering) 5 | ------------------------------------ 6 | - I2C SCL 19 7 | - I2C SDA 18 8 | 9 | - PPM in (receiver) 3 10 | 11 | - Rotor 1 22 12 | - Rotor 2 23 13 | - Rotor 3 9 14 | - Rotor 4 10 15 | - Rotor 5 6 16 | - Rotor 6 20 17 | - Rotor 7 21 18 | - Rotor 8 5 19 | 20 | - Orientation lights / Armed-Disarmed indicator 14 21 | 22 | - Battery Monitor (current sensor) 17 23 | 24 | Filters, kinematics, data handling 25 | ---------------------------------- 26 | - Initial raw data from sensors (read every 1ms = 1000Hz) is being averaged by a simple averaging filter 27 | - Averaged data from sensors are being processed every 10ms (100Hz) 28 | - CMP kinematitcs (my own) selected by default 29 | - ARG kinematics (from aeroquad) is also supported (can be enabled by simple include change) 30 | - DCM kinematics (from FreeIMU) 31 | - Flight controller supports 32 | - Rate (ACRO) | gyro only 33 | - Attitude | gyro with accel corrections 34 | - Altitute hold | barometer or sonar 35 | - Pilot commands are being read by PPM sampling code via single PIN (with HW timer), both PPM and PWM in is supported 36 | - Stabilization and pilot commands are mixed together by 2 separate PID controllers 37 | - First (only used in attitude mode) mixes pilot commands with kinematics output 38 | - Second (used in both attitude and rate mode) mixes output from first PID or raw stick input with gyroRate output 39 | - For ESC signal output i am using an build in 8 channel FLEX timer (yes you can controll octocopter with this) 40 | - ESC PWM signal supports both 250Hz and 400Hz update rate (running at 400Hz by default) 41 | 42 | Accelerometer offset trimming via TX 43 | ------------------------------------ 44 | - While in dis-armed state 45 | - To enter the trimming mode = Throttle stick UP & Rudder LEFT 46 | - To adjust the values = PITCH UP & DOWN || ROLL LEFT & RIGHT 47 | - To save the values in EEPROM = Throttle UP & Rudder RIGHT 48 | - (you dont have to save the values right away, you can test them, then return back to trimming mode and save them) 49 | - To leave trimming mode = Throttle stick DOWN -------------------------------------------------------------------------------- /controller/GPS.h: -------------------------------------------------------------------------------- 1 | class GPS_navigator { 2 | public: 3 | // Constructor 4 | GPS_navigator() { 5 | }; 6 | 7 | void initializeBaseStation() { 8 | // We need to get accurate fix, possible a while loop with accuracy check in the end 9 | // after this ground altitude should be saved 10 | }; 11 | 12 | void processPositionHold() { 13 | }; 14 | 15 | private: 16 | int32_t base_lat; 17 | int32_t base_lon; 18 | float base_ground_altitude; 19 | } gps; -------------------------------------------------------------------------------- /controller/PID.h: -------------------------------------------------------------------------------- 1 | /* Proportional–integral–derivative controller (PID controller) is a control loop feedback mechanism. 2 | 3 | PID controller calculates an "error" value as the difference between a measured process variable and a desired setpoint. 4 | The controller attempts to minimize the error by adjusting the process control inputs. 5 | 6 | Input = Kinematics or sensor input 7 | Output = PID output 8 | SetPoint = Desired output 9 | windupGuard = maximum and minimum constrain on I term 10 | 11 | Empty constructor will allow us to declare PID object globally anywhere before the setup() function 12 | This will allow us to feed PID settings and other variables when they are ready 13 | (in our case, after EEPROM has been initialized / read into variables) 14 | 15 | Pointer arithmetic used to parse terms into PID controller @http://www.cs.umd.edu/class/sum2003/cmsc311/Notes/BitOp/pointer.html 16 | */ 17 | class PID { 18 | public: 19 | // Constructor 20 | PID() { 21 | }; 22 | 23 | PID(float* Input, float* Output, float* Setpoint, float* terms) { 24 | previous_error = 0.0; 25 | integral = 0.0; 26 | 27 | PID_input = Input; 28 | PID_output = Output; 29 | PID_setpoint = Setpoint; 30 | 31 | Kp = &terms[P]; 32 | Ki = &terms[I]; 33 | Kd = &terms[D]; 34 | 35 | windupGuard = &terms[WG]; 36 | }; 37 | 38 | void Compute() { 39 | unsigned long now = micros(); 40 | float delta_time = (now - last_time) / 1000000.0; 41 | float error = *PID_setpoint - *PID_input; 42 | integral = constrain(integral + error * delta_time, -*windupGuard, *windupGuard); 43 | float derivative = (*PID_input - previous_error) / delta_time; 44 | 45 | *PID_output = *Kp * error + *Ki * integral + *Kd * derivative; 46 | 47 | previous_error = *PID_input; 48 | last_time = now; 49 | }; 50 | 51 | void IntegralReset() { 52 | integral = 0.0; 53 | }; 54 | 55 | private: 56 | float *PID_input; 57 | float *PID_output; 58 | float *PID_setpoint; 59 | 60 | float *Kp, *Ki, *Kd; 61 | float *windupGuard; 62 | 63 | float previous_error; 64 | float integral; 65 | unsigned long last_time; 66 | }; -------------------------------------------------------------------------------- /controller/controller.h: -------------------------------------------------------------------------------- 1 | // Main loop variables 2 | unsigned long currentTime = 0; 3 | unsigned long previousTime = 0; 4 | unsigned long sensorPreviousTime = 0; 5 | uint8_t frameCounter = 0; 6 | uint32_t itterations = 0; 7 | 8 | bool all_ready = false; 9 | bool armed = false; 10 | bool flightMode = false; 11 | bool altitudeHoldBaro = false; 12 | bool altitudeHoldSonar = false; 13 | bool positionHoldGPS = false; 14 | 15 | // Flight modes 16 | #define RATE_MODE 0 17 | #define ATTITUDE_MODE 1 18 | 19 | // Primary channel definitions 20 | #define ROLL 0 21 | #define PITCH 1 22 | #define THROTTLE 2 23 | #define YAW 3 24 | 25 | // PID pseudo definitions 26 | #define P 0 // Proportional 27 | #define I 1 // Integral 28 | #define D 2 // Derivative 29 | #define WG 3 // WindupGuard 30 | 31 | // Axis definitions 32 | #define XAXIS 0 33 | #define YAXIS 1 34 | #define ZAXIS 2 35 | 36 | // Arbitrary ON/OFF definitions 37 | #define OFF 0 38 | #define ON 1 39 | 40 | // Blinking LED to indicate activity 41 | bool Arduino_LED_state = 0; 42 | uint8_t Beacon_LED_state = 0; 43 | 44 | // Modulo definitions (integer remainder) 45 | #define TASK_50HZ 2 46 | #define TASK_10HZ 10 47 | #define TASK_1HZ 100 48 | 49 | // Kinematics variable defnitions 50 | float kinematicsAngle[3]; 51 | 52 | // FlightController commands definitions 53 | float commandYaw, commandYawAttitude, commandPitch, commandRoll, commandThrottle; 54 | 55 | // Heading related variables 56 | float headingError = 0.0; 57 | float headingSetpoint = 0.0; 58 | 59 | // PID variables 60 | float YawCommandPIDSpeed, PitchCommandPIDSpeed, RollCommandPIDSpeed; 61 | float YawMotorSpeed, PitchMotorSpeed, RollMotorSpeed, AltitudeHoldMotorSpeed; 62 | int16_t throttle = 1000; 63 | 64 | // +- PI normalization macro 65 | #define NORMALIZE(x) do { if ((x) < -PI) (x) += 2 * PI; else if ((x) > PI) (x) -= 2 * PI; } while (0); 66 | 67 | // Custom definitions 68 | //#define DISPLAY_ITTERATIONS -------------------------------------------------------------------------------- /controller/dataStorage.h: -------------------------------------------------------------------------------- 1 | /* EEPROM based configuration data storage structure 2 | 3 | Whole flight controller configuration is being stored inside an CONFIG structure 4 | that can be accesed as data union (which is required for easy manipulation with 5 | the configuration data over serial and easy way of storing this data in EEPROM). 6 | 7 | This method allow us to do "smart" memory checking with the new data against 8 | data stored in EEPROM, which means we doesn't have to re-write whole configuration 9 | union inside the EEPROM, but we just modify bytes that changed. 10 | 11 | This will protect EEPROM from unnecessary writes, extending its lifetime 12 | (because EEPROM writes are limited, actual number of writes depends on the chip). 13 | */ 14 | 15 | #if defined(__MK20DX128__) 16 | #define EEPROM_SIZE 512 17 | #endif 18 | 19 | #define EEPROM_VERSION 5 20 | 21 | struct AXIS_MAP_struct { 22 | uint8_t axis1:2; 23 | uint8_t axis1_sign:1; 24 | uint8_t axis2:2; 25 | uint8_t axis2_sign:1; 26 | uint8_t axis3:2; 27 | uint8_t axis3_sign:1; 28 | uint8_t initialized:1; 29 | }; 30 | 31 | struct __attribute__((packed)) CONFIG_struct { 32 | uint8_t version; 33 | bool calibrateESC; 34 | uint16_t minimumArmedThrottle; 35 | 36 | // Sensor axis map 37 | struct AXIS_MAP_struct GYRO_AXIS_MAP; 38 | struct AXIS_MAP_struct ACCEL_AXIS_MAP; 39 | struct AXIS_MAP_struct MAG_AXIS_MAP; 40 | 41 | // Accelerometer 42 | int16_t ACCEL_BIAS[3]; 43 | 44 | // RX 45 | uint8_t CHANNEL_ASSIGNMENT[16]; 46 | uint64_t CHANNEL_FUNCTIONS[4]; 47 | 48 | // Attitude 49 | float PID_YAW_c[4]; 50 | float PID_PITCH_c[4]; 51 | float PID_ROLL_c[4]; 52 | 53 | // Rate 54 | float PID_YAW_m[4]; 55 | float PID_PITCH_m[4]; 56 | float PID_ROLL_m[4]; 57 | 58 | float PID_BARO[4]; 59 | float PID_SONAR[4]; 60 | 61 | // GPS 62 | float PID_GPS[4]; 63 | }; 64 | 65 | union CONFIG_union { 66 | struct CONFIG_struct data; 67 | uint8_t raw[sizeof(struct CONFIG_struct)]; 68 | }; 69 | 70 | CONFIG_union CONFIG; 71 | 72 | void initializeEEPROM(void) { 73 | // Default settings should be initialized here 74 | CONFIG.data.version = EEPROM_VERSION; 75 | CONFIG.data.calibrateESC = 0; 76 | CONFIG.data.minimumArmedThrottle = 1100; 77 | 78 | // Accelerometer 79 | CONFIG.data.ACCEL_BIAS[XAXIS] = 0; 80 | CONFIG.data.ACCEL_BIAS[YAXIS] = 0; 81 | CONFIG.data.ACCEL_BIAS[ZAXIS] = 0; 82 | 83 | // RX 84 | CONFIG.data.CHANNEL_ASSIGNMENT[0] = 0; 85 | CONFIG.data.CHANNEL_ASSIGNMENT[1] = 1; 86 | CONFIG.data.CHANNEL_ASSIGNMENT[2] = 2; 87 | CONFIG.data.CHANNEL_ASSIGNMENT[3] = 3; 88 | CONFIG.data.CHANNEL_ASSIGNMENT[4] = 4; 89 | CONFIG.data.CHANNEL_ASSIGNMENT[5] = 5; 90 | CONFIG.data.CHANNEL_ASSIGNMENT[6] = 6; 91 | CONFIG.data.CHANNEL_ASSIGNMENT[7] = 7; 92 | CONFIG.data.CHANNEL_ASSIGNMENT[8] = 8; 93 | CONFIG.data.CHANNEL_ASSIGNMENT[9] = 9; 94 | CONFIG.data.CHANNEL_ASSIGNMENT[10] = 10; 95 | CONFIG.data.CHANNEL_ASSIGNMENT[11] = 11; 96 | CONFIG.data.CHANNEL_ASSIGNMENT[12] = 12; 97 | CONFIG.data.CHANNEL_ASSIGNMENT[13] = 13; 98 | CONFIG.data.CHANNEL_ASSIGNMENT[14] = 14; 99 | CONFIG.data.CHANNEL_ASSIGNMENT[15] = 15; 100 | 101 | CONFIG.data.CHANNEL_FUNCTIONS[0] = 0x04; // mode select ("stable mode" is set to trigger on AUX1-HIGH by default) 102 | CONFIG.data.CHANNEL_FUNCTIONS[1] = 0x00; // baro select 103 | CONFIG.data.CHANNEL_FUNCTIONS[2] = 0x00; // sonar select 104 | CONFIG.data.CHANNEL_FUNCTIONS[3] = 0x00; // GPS select 105 | 106 | // Altitude 107 | CONFIG.data.PID_YAW_c[P] = 4.0; 108 | CONFIG.data.PID_YAW_c[I] = 0.0; 109 | CONFIG.data.PID_YAW_c[D] = 0.0; 110 | CONFIG.data.PID_YAW_c[WG] = 25.0; 111 | 112 | CONFIG.data.PID_PITCH_c[P] = 4.0; 113 | CONFIG.data.PID_PITCH_c[I] = 0.0; 114 | CONFIG.data.PID_PITCH_c[D] = 0.0; 115 | CONFIG.data.PID_PITCH_c[WG] = 25.0; 116 | 117 | CONFIG.data.PID_ROLL_c[P] = 4.0; 118 | CONFIG.data.PID_ROLL_c[I] = 0.0; 119 | CONFIG.data.PID_ROLL_c[D] = 0.0; 120 | CONFIG.data.PID_ROLL_c[WG] = 25.0; 121 | 122 | // Rate 123 | CONFIG.data.PID_YAW_m[P] = 200.0; 124 | CONFIG.data.PID_YAW_m[I] = 5.0; 125 | CONFIG.data.PID_YAW_m[D] = 0.0; 126 | CONFIG.data.PID_YAW_m[WG] = 100.0; 127 | 128 | CONFIG.data.PID_PITCH_m[P] = 80.0; 129 | CONFIG.data.PID_PITCH_m[I] = 0.0; 130 | CONFIG.data.PID_PITCH_m[D] = -3.0; 131 | CONFIG.data.PID_PITCH_m[WG] = 1000.0; 132 | 133 | CONFIG.data.PID_ROLL_m[P] = 80.0; 134 | CONFIG.data.PID_ROLL_m[I] = 0.0; 135 | CONFIG.data.PID_ROLL_m[D] = -3.0; 136 | CONFIG.data.PID_ROLL_m[WG] = 1000.0; 137 | 138 | // Baro 139 | CONFIG.data.PID_BARO[P] = 25.0; 140 | CONFIG.data.PID_BARO[I] = 0.6; 141 | CONFIG.data.PID_BARO[D] = -1.0; 142 | CONFIG.data.PID_BARO[WG] = 25.0; 143 | 144 | // Sonar 145 | CONFIG.data.PID_SONAR[P] = 50.0; 146 | CONFIG.data.PID_SONAR[I] = 0.6; 147 | CONFIG.data.PID_SONAR[D] = -1.0; 148 | CONFIG.data.PID_SONAR[WG] = 25.0; 149 | 150 | // GPS 151 | CONFIG.data.PID_GPS[P] = 0.0; 152 | CONFIG.data.PID_GPS[I] = 0.0; 153 | CONFIG.data.PID_GPS[D] = 0.0; 154 | CONFIG.data.PID_GPS[WG] = 0.0; 155 | 156 | // This function will only initialize data variables 157 | // writeEEPROM() needs to be called manually to store this data in EEPROM 158 | } 159 | 160 | void writeEEPROM(void) { 161 | for (uint16_t i = 0; i < sizeof(struct CONFIG_struct); i++) { 162 | if (CONFIG.raw[i] != EEPROM.read(i)) { 163 | // Only re-write new data 164 | // blocks containing the same value will be left alone 165 | EEPROM.write(i, CONFIG.raw[i]); 166 | } 167 | } 168 | } 169 | 170 | void readEEPROM(void) { 171 | if (EEPROM.read(0) == 255) { 172 | // No EEPROM values detected, re-initialize 173 | initializeEEPROM(); 174 | } else { 175 | // There "is" data in the EEPROM, read it all 176 | for (uint16_t i = 0; i < sizeof(struct CONFIG_struct); i++) { 177 | CONFIG.raw[i] = EEPROM.read(i); 178 | } 179 | 180 | // Verify version 181 | if (CONFIG.data.version != EEPROM_VERSION) { 182 | // Version doesn't match, re-initialize 183 | initializeEEPROM(); 184 | } 185 | } 186 | } 187 | -------------------------------------------------------------------------------- /controller/math.h: -------------------------------------------------------------------------------- 1 | // Fast inverse square root implementation 2 | // @see: http://en.wikipedia.org/wiki/Fast_inverse_square_root 3 | float invSqrt(float number) { 4 | long i; 5 | float x, y; 6 | const float f = 1.5F; 7 | 8 | x = number * 0.5F; 9 | y = number; 10 | i = * ( long * ) &y; 11 | i = 0x5f375a86 - ( i >> 1 ); // older version with less accuracy = 0x5f3759df 12 | y = * ( float * ) &i; 13 | y = y * ( f - ( x * y * y ) ); 14 | 15 | return y; 16 | } 17 | 18 | // Signed square root implementation 19 | float s_sqrt(float val) { 20 | if (val < 0.00) { 21 | // negative input = negative sqrt 22 | return -sqrt(abs(val)); 23 | } 24 | 25 | return sqrt(val); 26 | } 27 | 28 | // Smooth Filter 29 | float filterSmooth(float currentData, float previousData, float smoothFactor) { 30 | return (previousData * (1.0 - smoothFactor) + (currentData * smoothFactor)); 31 | } -------------------------------------------------------------------------------- /controller/sensors.h: -------------------------------------------------------------------------------- 1 | #define GYROSCOPE_DETECTED 0x01 2 | #define ACCELEROMETER_DETECTED 0x02 3 | #define MAGNETOMETER_DETECTED 0x04 4 | #define BAROMETER_DETECTED 0x08 5 | #define GPS_DETECTED 0x10 6 | 7 | class SensorArray { 8 | public: 9 | uint16_t sensors_detected; 10 | 11 | SensorArray() { 12 | sensors_detected = 0x00; 13 | }; 14 | 15 | void initializeGyro(); 16 | void readGyroSum(); 17 | void evaluateGyro(); 18 | void initializeAccel(); 19 | void calibrateAccel(); 20 | void readAccelSum(); 21 | void evaluateAccel(); 22 | void initializeMag(); 23 | void readMag(); 24 | void evaluateMag(); 25 | void initializeBaro(); 26 | void readBaroSum(); 27 | void evaluateBaroAltitude(); 28 | void initializeGPS(); 29 | void readGPS(); 30 | void evaluateGPS(); 31 | 32 | // I2C stuff 33 | void i2c_write8 (int16_t deviceAddress, uint8_t registerAddress, int16_t registerValue) { 34 | // I am using int16_t for dataValue becuase we don't know if we are writing 35 | // 8 bit signed or unsigned value. 36 | 37 | Wire.beginTransmission(deviceAddress); 38 | Wire.write(registerAddress); 39 | Wire.write(registerValue); 40 | Wire.endTransmission(); 41 | }; 42 | 43 | int16_t i2c_read16 (int16_t deviceAddress, uint8_t registerAddress) { 44 | int16_t data; 45 | 46 | Wire.beginTransmission(deviceAddress); 47 | Wire.write(registerAddress); 48 | Wire.endTransmission(); 49 | 50 | Wire.requestFrom(deviceAddress, 2); 51 | 52 | data = (Wire.read() << 8) | Wire.read(); 53 | 54 | return data; 55 | }; 56 | }; 57 | 58 | SensorArray sensors; 59 | -------------------------------------------------------------------------------- /extras/Configurator/background.js: -------------------------------------------------------------------------------- 1 | chrome.app.runtime.onLaunched.addListener(function() { 2 | chrome.app.window.create('main.html', { 3 | frame: 'chrome', 4 | id: 'main-window', 5 | minWidth: 960, 6 | maxWidth: 960, 7 | minHeight: 550, 8 | maxHeight: 550 9 | }, function(main_window) { 10 | main_window.onClosed.addListener(function() { 11 | // connectionId is passed from the script side through the chrome.runtime.getBackgroundPage refference 12 | // allowing us to automatically close the port when application shut down 13 | if (connectionId != -1) { 14 | chrome.serial.close(connectionId, function() { 15 | console.log('CLEANUP: Connection to serial port was opened after application closed, closing the connection.'); 16 | }); 17 | } 18 | }); 19 | }); 20 | }); -------------------------------------------------------------------------------- /extras/Configurator/js/data_storage.js: -------------------------------------------------------------------------------- 1 | var sensors_alive = { 2 | gyro: 0, 3 | accel: 0, 4 | mag: 0, 5 | baro: 0, 6 | gps: 0 7 | } 8 | 9 | var AUX_triggered_mask = [0, 0]; 10 | 11 | var sensor_data = { 12 | gyro: [0, 0, 0], 13 | accel: [0, 0, 0], 14 | mag: [0, 0, 0], 15 | baro: [0, 0], 16 | kinematics: [0, 0, 0], 17 | 18 | // plot data 19 | gyro_plot: [new Array(), new Array(), new Array()], 20 | accel_plot: [new Array(), new Array(), new Array()], 21 | mag_plot: [new Array(), new Array(), new Array()], 22 | baro_plot: new Array() 23 | } 24 | 25 | var receiver_data = { 26 | raw: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 27 | plot: [ 28 | new Array(), new Array(), new Array(), new Array(), new Array(), new Array(), new Array(), new Array(), 29 | new Array(), new Array(), new Array(), new Array(), new Array(), new Array(), new Array(), new Array() 30 | ] 31 | } 32 | 33 | var motors = 0; 34 | var motors_output = [0, 0, 0, 0, 0, 0, 0, 0]; 35 | 36 | var GPS_data = { 37 | lat: 0, 38 | lon: 0, 39 | course: 0, 40 | speed: 0, 41 | height: 0, 42 | accuracy: 0, 43 | fixtime: 0, 44 | sentences: 0, 45 | state: 0, 46 | sats: 0 47 | } -------------------------------------------------------------------------------- /extras/Configurator/js/file_system.js: -------------------------------------------------------------------------------- 1 | function configuration_backup() { 2 | var chosenFileEntry = null; 3 | 4 | var accepts = [{ 5 | extensions: ['txt'] 6 | }]; 7 | 8 | // generate timestamp for the backup file 9 | var d = new Date(); 10 | var now = d.getUTCFullYear() + '.' + d.getDate() + '.' + (d.getMonth() + 1) + '.' + d.getHours() + '.' + d.getMinutes(); 11 | 12 | // create or load the file 13 | chrome.fileSystem.chooseEntry({type: 'saveFile', suggestedName: 'phoenix_backup_' + now, accepts: accepts}, function(fileEntry) { 14 | if (!fileEntry) { 15 | command_log('No file selected'); 16 | console.log('No file selected'); 17 | 18 | return; 19 | } 20 | 21 | chosenFileEntry = fileEntry; 22 | 23 | // echo/console log path specified 24 | chrome.fileSystem.getDisplayPath(chosenFileEntry, function(path) { 25 | command_log('Backup file path: ' + path); 26 | console.log('Backup file path: ' + path); 27 | }); 28 | 29 | // change file entry from read only to read/write 30 | chrome.fileSystem.getWritableEntry(chosenFileEntry, function(fileEntryWritable) { 31 | // check if file is writable 32 | chrome.fileSystem.isWritableEntry(fileEntryWritable, function(isWritable) { 33 | if (isWritable) { 34 | chosenFileEntry = fileEntryWritable; 35 | 36 | // crunch the config object 37 | var serialized_config_object = JSON.stringify(eepromConfig); 38 | var blob = new Blob([serialized_config_object], {type: 'text/plain'}); // first parameter for Blob needs to be an array 39 | 40 | chosenFileEntry.createWriter(function(writer) { 41 | writer.onerror = function (e) { 42 | console.error(e); 43 | }; 44 | 45 | writer.onwriteend = function() { 46 | command_log('Write -- SUCCESSFUL'); 47 | console.log('Write SUCCESSFUL'); 48 | }; 49 | 50 | //writer.truncate(blob.size); 51 | writer.write(blob); 52 | }, function (e) { 53 | console.error(e); 54 | }); 55 | } else { 56 | // Something went wrong or file is set to read only and cannot be changed 57 | command_log('File appears to be read ONLY'); 58 | console.log('File appears to be read only, sorry.'); 59 | } 60 | }); 61 | }); 62 | }); 63 | } 64 | 65 | function configuration_restore() { 66 | var chosenFileEntry = null; 67 | 68 | var accepts = [{ 69 | extensions: ['txt'] 70 | }]; 71 | 72 | // load up the file 73 | chrome.fileSystem.chooseEntry({type: 'openFile', accepts: accepts}, function(fileEntry) { 74 | if (!fileEntry) { 75 | command_log('No file selected'); 76 | console.log('No file selected'); 77 | 78 | return; 79 | } 80 | 81 | chosenFileEntry = fileEntry; 82 | 83 | // echo/console log path specified 84 | chrome.fileSystem.getDisplayPath(chosenFileEntry, function(path) { 85 | command_log('Restore file path: ' + path); 86 | console.log('Restore file path: ' + path); 87 | }); 88 | 89 | // read contents into variable 90 | chosenFileEntry.file(function(file) { 91 | var reader = new FileReader(); 92 | 93 | reader.onerror = function (e) { 94 | console.error(e); 95 | }; 96 | 97 | reader.onloadend = function(e) { 98 | command_log('Read SUCCESSFUL'); 99 | console.log('Read SUCCESSFUL'); 100 | 101 | try { // check if string provided is a valid JSON 102 | var deserialized_config_object = JSON.parse(e.target.result); 103 | 104 | if (deserialized_config_object.version === undefined) throw 'no version parameter found'; 105 | } catch (e) { 106 | // data provided != valid json object 107 | command_log('Data provided doesn\'t contain valid JSON string -- ABORTING'); 108 | console.log('Data provided != valid JSON string'); 109 | 110 | return; 111 | } 112 | 113 | // replacing "old configuration" with configuration from backup file 114 | if (eepromConfig.version == deserialized_config_object.version) { 115 | command_log('EEPROM version number/pattern -- MATCH'); 116 | console.log('EEPROM version number/pattern matches backup EEEPROM file version/pattern'); 117 | } else { 118 | command_log('EEPROM version number/pattern -- MISSMATCH (manual values re-validation is advised)'); 119 | console.log('EEPROM version number/pattern doesn\'t match backup EEPROM file version/pattern (manual values re-validation is advised)'); 120 | } 121 | 122 | eepromConfig = deserialized_config_object; 123 | 124 | // Send updated UNION to the flight controller 125 | sendUNION(); 126 | 127 | // Refresh Initial UI 128 | $('#content').load("./tabs/initial_setup.html", tab_initialize_initial_setup); 129 | }; 130 | 131 | reader.readAsText(file); 132 | }); 133 | }); 134 | } -------------------------------------------------------------------------------- /extras/Configurator/js/union_process.js: -------------------------------------------------------------------------------- 1 | /* Code below contains an exact representation of eeprom configuration array 2 | that is stored inside the flight controller. 3 | 4 | Rest of the code extends the DataView low-level interface, that is now included 5 | by default inside the javascript engine (chrome v9, firefox v15, IE v10). 6 | 7 | You can read more about the DataView interface and its methods 8 | @ https://developer.mozilla.org/en-US/docs/JavaScript/Typed_arrays/DataView 9 | */ 10 | 11 | var eepromConfig = { 12 | version: 0, 13 | calibrateESC: 0, 14 | minimumArmedThrottle: 0, 15 | 16 | SENSOR_AXIS_MAP: [0, 0, 0], 17 | 18 | ACCEL_BIAS: [0, 0, 0], 19 | 20 | CHANNEL_ASSIGNMENT: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 21 | CHANNEL_FUNCTIONS: [[0, 0], [0, 0], [0, 0], [0, 0]], 22 | 23 | PID_YAW_c: [0.0, 0.0, 0.0, 0], 24 | PID_PITCH_c: [0.0, 0.0, 0.0, 0], 25 | PID_ROLL_c: [0.0, 0.0, 0.0, 0], 26 | 27 | PID_YAW_m: [0.0, 0.0, 0.0, 0], 28 | PID_PITCH_m: [0.0, 0.0, 0.0, 0], 29 | PID_ROLL_m: [0.0, 0.0, 0.0, 0], 30 | 31 | PID_BARO: [0.0, 0.0, 0.0, 0], 32 | PID_SONAR: [0.0, 0.0, 0.0, 0], 33 | PID_GPS: [0.0, 0.0, 0.0, 0], 34 | }; 35 | var eepromConfigSize; 36 | 37 | DataView.prototype.getString = function(str_length, byteOffset) { 38 | var value = ''; 39 | for (var i = 0; i < str_length; ++i) { 40 | var char = this.getUint8(byteOffset + i); 41 | value += String.fromCharCode(char > 127 ? 65533 : char); 42 | } 43 | 44 | return value; 45 | } 46 | 47 | DataView.prototype.getChar = function(byteOffset) { 48 | return this.getString(1, byteOffset); 49 | } 50 | 51 | DataView.prototype.parseUNION = function (structure) { 52 | var needle = 0; 53 | 54 | structure.version = this.getUint8(needle++); 55 | structure.calibrateESC = this.getUint8(needle++); 56 | structure.minimumArmedThrottle = this.getUint16(needle, 1); 57 | needle += 2; 58 | 59 | for (var i = 0; i < structure.SENSOR_AXIS_MAP.length; i++) { 60 | structure.SENSOR_AXIS_MAP[i] = this.getInt16(needle, 1); 61 | needle += 2; 62 | } 63 | 64 | for (var i = 0; i < structure.ACCEL_BIAS.length; i++) { 65 | structure.ACCEL_BIAS[i] = this.getInt16(needle, 1); 66 | needle += 2; 67 | } 68 | 69 | for (var i = 0; i < structure.CHANNEL_ASSIGNMENT.length; i++) { 70 | structure.CHANNEL_ASSIGNMENT[i] = this.getUint8(needle, 1); 71 | needle += 1; 72 | } 73 | 74 | for (var i = 0; i < structure.CHANNEL_FUNCTIONS.length; i++) { 75 | structure.CHANNEL_FUNCTIONS[i][0] = this.getUint32(needle, 1); 76 | structure.CHANNEL_FUNCTIONS[i][1] = this.getUint32(needle + 4, 1); 77 | needle += 8; 78 | } 79 | 80 | for (var i = 0; i < 4; i++) { 81 | structure.PID_YAW_c[i] = this.getFloat32(needle, 1); 82 | structure.PID_PITCH_c[i] = this.getFloat32(needle + 16, 1); 83 | structure.PID_ROLL_c[i] = this.getFloat32(needle + 32, 1); 84 | 85 | structure.PID_YAW_m[i] = this.getFloat32(needle + 48, 1); 86 | structure.PID_PITCH_m[i] = this.getFloat32(needle + 64, 1); 87 | structure.PID_ROLL_m[i] = this.getFloat32(needle + 80, 1); 88 | 89 | structure.PID_BARO[i] = this.getFloat32(needle + 96, 1); 90 | structure.PID_SONAR[i] = this.getFloat32(needle + 112, 1); 91 | structure.PID_GPS[i] = this.getFloat32(needle + 128, 1); 92 | 93 | needle += 4; 94 | } 95 | } 96 | 97 | DataView.prototype.setUNION = function (structure) { 98 | var needle = 0; 99 | 100 | this.setUint8(needle++, structure.version); 101 | this.setUint8(needle++, structure.calibrateESC); 102 | this.setUint16(needle, structure.minimumArmedThrottle, 1); 103 | needle += 2; 104 | 105 | for (var i = 0; i < structure.SENSOR_AXIS_MAP.length; i++) { 106 | this.setInt16(needle, structure.SENSOR_AXIS_MAP[i], 1); 107 | needle += 2; 108 | } 109 | 110 | for (var i = 0; i < structure.ACCEL_BIAS.length; i++) { 111 | this.setInt16(needle, structure.ACCEL_BIAS[i], 1); 112 | needle += 2; 113 | } 114 | 115 | for (var i = 0; i < structure.CHANNEL_ASSIGNMENT.length; i++) { 116 | this.setUint8(needle, structure.CHANNEL_ASSIGNMENT[i], 1); 117 | needle += 1; 118 | } 119 | 120 | for (var i = 0; i < structure.CHANNEL_FUNCTIONS.length; i++) { 121 | this.setUint32(needle, structure.CHANNEL_FUNCTIONS[i][0], 1); 122 | this.setUint32(needle + 4, structure.CHANNEL_FUNCTIONS[i][1], 1); 123 | needle += 8; 124 | } 125 | 126 | for (var i = 0; i < 4; i++) { 127 | this.setFloat32(needle, structure.PID_YAW_c[i], 1); 128 | this.setFloat32(needle + 16, structure.PID_PITCH_c[i], 1); 129 | this.setFloat32(needle + 32, structure.PID_ROLL_c[i], 1); 130 | 131 | this.setFloat32(needle + 48, structure.PID_YAW_m[i], 1); 132 | this.setFloat32(needle + 64, structure.PID_PITCH_m[i], 1); 133 | this.setFloat32(needle + 80, structure.PID_ROLL_m[i], 1); 134 | 135 | this.setFloat32(needle + 96, structure.PID_BARO[i], 1); 136 | this.setFloat32(needle + 112, structure.PID_SONAR[i], 1); 137 | this.setFloat32(needle + 128, structure.PID_GPS[i], 1); 138 | 139 | needle += 4; 140 | } 141 | } 142 | 143 | function requestUNION() { 144 | command_log('Requesting configuration UNION from Flight Controller'); 145 | send_message(PSP.PSP_REQ_CONFIGURATION, 1); 146 | } 147 | 148 | function sendUNION() { 149 | command_log('Sending Configuration UNION to Flight Controller ...'); 150 | 151 | var eepromConfigBytes = new ArrayBuffer(eepromConfigSize); 152 | var view = new DataView(eepromConfigBytes, 0); 153 | view.setUNION(eepromConfig); 154 | 155 | var data = new Uint8Array(eepromConfigBytes); 156 | send_message(PSP.PSP_SET_CONFIGURATION, data); 157 | } -------------------------------------------------------------------------------- /extras/Configurator/main.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Phoenix Configurator 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 | 72 |
73 |
74 | 81 |
82 |
83 |
84 | Port utilization: 0 % 85 |
86 |
87 |
88 |
89 |
90 |
91 |
92 | 105 |
106 |
107 |
108 |
109 |
110 | 111 | -------------------------------------------------------------------------------- /extras/Configurator/manifest.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Phoenix Configurator", 3 | "description": "Application required to change Phoenix configuration stored in EEPROM", 4 | "version": "0.1", 5 | "manifest_version": 2, 6 | "minimum_chrome_version": "23", 7 | 8 | "app": { 9 | "background": { 10 | "scripts": [ "background.js" ] 11 | } 12 | }, 13 | 14 | "permissions": [ 15 | "serial", 16 | "storage", 17 | "fileSystem", 18 | "fileSystem.write" 19 | ] 20 | } -------------------------------------------------------------------------------- /extras/Configurator/readme.txt: -------------------------------------------------------------------------------- 1 | Ubuntu (and ubuntu distro based) users might need to add their selfs to the dialout group 2 | otherwise they would not have enough permissions to join the serial bus and configurator 3 | would just end up returning an "Could not join the serial bus" error. 4 | sudo usermod -aG dialout YOUR_USERNAME -------------------------------------------------------------------------------- /extras/Configurator/tabs/about.html: -------------------------------------------------------------------------------- 1 |
2 |

3 | Phoenix Configurator was designed to improve user experience by not requiring extensive programming knowledge
4 | needed to do proper sensor calibration and debugging of various flight control elements (kinematics, motor outputs, etc).
5 |
6 | This also allows users to do PID tuning right from the configurator (without the need to reflash the flight controller afterwards).
7 | Because all of the configuration changes are executed on the flight controllers side in real time.
8 |
9 |

10 |

11 | People that supported the Phoenix project (with money, hardware, or otherwise) [alphabetical]
12 |
13 |

23 |

24 |

25 |
26 | Created by cTn 27 |

28 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/channel_assigner.html: -------------------------------------------------------------------------------- 1 |
2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 |
NameAssigned ChannelAction
ROLLUpdate
PITCHUpdate
THROTTLEUpdate
YAW/RUDDERUpdate
AUX 1Update
AUX 2Update
AUX 3Update
AUX 4Update
AUX 5Update
AUX 6Update
AUX 7Update
AUX 8Update
AUX 9Update
AUX 10Update
AUX 11Update
AUX 12Update
89 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/channel_assigner.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_channel_assigner() { 2 | var i = 0; 3 | 4 | $('#content table.channel_assigner input').each(function() { 5 | $(this).val(eepromConfig.CHANNEL_ASSIGNMENT[i++]); 6 | }); 7 | 8 | // UI hooks 9 | $('#content table.channel_assigner input').focus(function() { 10 | $(this).data('prev_val', $(this).val()); 11 | $(this).val(''); // empty input field 12 | }); 13 | 14 | $('#content table.channel_assigner input').blur(function() { 15 | if ($(this).val() == '') { // if input field is empty restore previous value 16 | $(this).val($(this).data('prev_val')); 17 | } 18 | }); 19 | 20 | $('#content table.channel_assigner a.update').click(function() { 21 | var parent = $(this).parent().parent(); 22 | 23 | // - 1 because .index() starts at 1 not 0 24 | eepromConfig.CHANNEL_ASSIGNMENT[parent.index() - 1] = parseInt($('input', parent).val()); 25 | 26 | // Send updated UNION to the flight controller 27 | sendUNION(); 28 | }); 29 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/function_triggers.html: -------------------------------------------------------------------------------- 1 |
2 |
3 | 17 |
18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 |
NameLMHLMHLMHLMHLMHLMHLMHLMHLMHLMHLMHLMH
70 | Update 71 |
72 |

73 | Options with orange background color indicate current AUX channel value.
74 | If you are unsure about how this works, try flipping the AUX switches on your Transmitter and watch how the output / background 75 | changes on the screen.
76 |

77 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/function_triggers.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_function_triggers() { 2 | // AUX names manually defined 3 | var AUX_names = [ 4 | 'Stable mode (gyro + acc)', 5 | 'Altitude hold (barometer)', 6 | 'Altitude hold (sonar)', 7 | 'GPS (not implemented)' 8 | ]; 9 | 10 | // generate table from the supplied AUX names and AUX data 11 | for (var i = 0; i < eepromConfig.CHANNEL_FUNCTIONS.length; i++) { 12 | $('.tab-function_triggers .functions > tbody:last').append( 13 | '' + 14 | '' + AUX_names[i] + '' + 15 | // AUX 1 16 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 0) + 17 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 1) + 18 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 2) + 19 | 20 | // AUX 2 21 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 3) + 22 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 4) + 23 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 5) + 24 | 25 | // AUX 3 26 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 6) + 27 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 7) + 28 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 8) + 29 | 30 | // AUX 4 31 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 9) + 32 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 10) + 33 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 11) + 34 | 35 | // AUX 5 36 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 12) + 37 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 13) + 38 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 14) + 39 | 40 | // AUX 6 41 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 15) + 42 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 16) + 43 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 17) + 44 | 45 | // AUX 7 46 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 18) + 47 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 19) + 48 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 20) + 49 | 50 | // AUX 8 51 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 21) + 52 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 22) + 53 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 23) + 54 | 55 | // AUX 9 56 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 24) + 57 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 25) + 58 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 26) + 59 | 60 | // AUX 10 61 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 27) + 62 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 28) + 63 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 29) + 64 | 65 | // AUX 11 66 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 30) + 67 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][0], 31) + 68 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][1], 32) + 69 | 70 | // AUX 12 71 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][1], 33) + 72 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][1], 34) + 73 | box_check(eepromConfig.CHANNEL_FUNCTIONS[i][1], 35) + 74 | '' 75 | ); 76 | } 77 | 78 | $('.tab-function_triggers a.update').click(function() { 79 | // catch the input changes 80 | var main_needle = 0; 81 | var needle = 0; 82 | $('.tab-function_triggers .functions input').each(function() { 83 | if ($(this).is(':checked')) { 84 | if (needle < 32) { 85 | eepromConfig.CHANNEL_FUNCTIONS[main_needle][0] = bit_set(eepromConfig.CHANNEL_FUNCTIONS[main_needle][0], needle); 86 | } else if (needle >= 32 && needle < 64) { 87 | eepromConfig.CHANNEL_FUNCTIONS[main_needle][1] = bit_set(eepromConfig.CHANNEL_FUNCTIONS[main_needle][1], needle - 32); 88 | } 89 | } else { 90 | if (needle < 32) { 91 | eepromConfig.CHANNEL_FUNCTIONS[main_needle][0] = bit_clear(eepromConfig.CHANNEL_FUNCTIONS[main_needle][0], needle); 92 | } else if (needle >= 32 && needle < 64) { 93 | eepromConfig.CHANNEL_FUNCTIONS[main_needle][1] = bit_clear(eepromConfig.CHANNEL_FUNCTIONS[main_needle][1], needle - 32); 94 | } 95 | } 96 | 97 | needle++; 98 | 99 | if (needle >= 36) { // 12 aux * 3 checkboxes = 36 bits per line 100 | main_needle++; 101 | 102 | needle = 0; 103 | } 104 | }); 105 | 106 | // Send updated UNION to the flight controller 107 | sendUNION(); 108 | }); 109 | 110 | // request AUX mask from flight controller 111 | timers.push(setInterval(AUX_pull, 50)); 112 | } 113 | 114 | function AUX_pull() { 115 | // Update the classes 116 | var needle = 0; 117 | $('.tab-function_triggers .functions th:not(.name)').each(function() { 118 | if (needle < 32) { 119 | if (bit_check(AUX_triggered_mask[0], needle)) { // 1 120 | $(this).addClass('on'); 121 | } else { // 0 122 | $(this).removeClass('on'); 123 | } 124 | } else if (needle >= 32 && needle < 64) { 125 | if (bit_check(AUX_triggered_mask[1], needle)) { // 1 126 | $(this).addClass('on'); 127 | } else { // 0 128 | $(this).removeClass('on'); 129 | } 130 | } 131 | 132 | needle++; 133 | }); 134 | 135 | 136 | // request new data 137 | send_message(PSP.PSP_REQ_AUX_TRIGGERED, 1); 138 | } 139 | 140 | function box_check(num, pos) { 141 | if (bit_check(num, pos)) { // 1 142 | return ''; 143 | } else { // 0 144 | return ''; 145 | } 146 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/gps.html: -------------------------------------------------------------------------------- 1 |
2 |
3 |
GPS info
4 |
5 |
Latitude:
0
6 |
Longitude:
0
7 |
Speed:
0
8 |
Height:
0
9 |
Accuracy:
0
10 |
Heading:
0
11 |
State:
0
12 |
Satellites:
0
13 |
14 |
15 |
16 |
GPS Heading
17 |
18 | N 19 | E 20 | S 21 | W 22 |
23 | 24 | 25 |
26 |
27 |
28 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/gps.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_gps() { 2 | 3 | // start pulling gps data (only if gps was detected 4 | if (sensors_alive.gps) { 5 | timers.push(setInterval(gps_pull, 100)); // 10 Hz - more then enough, as most of the modules run on 1-5Hz 6 | } 7 | } 8 | 9 | function gps_pull() { 10 | // update UI with latest data 11 | var e_inf = $('.tab-gps .gps-info dl'); 12 | $('dd:eq(0)', e_inf).html((GPS_data.lat).toFixed(4) + ' deg'); 13 | $('dd:eq(1)', e_inf).html((GPS_data.lon).toFixed(4) + ' deg'); 14 | $('dd:eq(2)', e_inf).html((GPS_data.speed).toFixed(2) + ' km / h'); 15 | $('dd:eq(3)', e_inf).html(GPS_data.height + ' m'); 16 | $('dd:eq(4)', e_inf).html((GPS_data.accuracy).toFixed(2) + ' m'); 17 | $('dd:eq(5)', e_inf).html((GPS_data.course).toFixed(0) + ' deg'); 18 | 19 | if (GPS_data.state == 1) { // no fix 20 | $('dd:eq(6)', e_inf).html('No fix'); 21 | } else if (GPS_data.state == 2) { // 2D fix 22 | $('dd:eq(6)', e_inf).html('2D fix'); 23 | } else if (GPS_data.state == 3) { // 3D fix 24 | $('dd:eq(6)', e_inf).html('3D fix'); 25 | } 26 | 27 | $('dd:eq(7)', e_inf).html(GPS_data.sats); // number of satellites 28 | 29 | $('div.compass .pointer').css('-webkit-transform', 'rotate(' + (GPS_data.course) + 'deg)'); 30 | 31 | // request new data 32 | send_message(PSP.PSP_REQ_GPS, 1); 33 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/initial_setup.html: -------------------------------------------------------------------------------- 1 |
2 |
3 | Calibrate ESCs 4 |

5 | Requesting ESC calibration will result in ESC throttle range calibration on next flight controller startup.
6 | Its recommended to remove propellers before calibrating (for safety).
7 |

8 |
9 |
10 |
11 |
12 | Calibrate Accelerometer 13 |
    14 |
  • X
    • 15 |
  • Y
    • 16 |
  • Z
    • 17 |
18 |
    19 |
    • 20 |
    • 21 |
    • 22 |
23 | Manual Update 24 |
25 |

26 | Place your UAV on a flat surface (properly leveled desk works great) and click on the calibration button.
27 | Try not to bump into the desk while calibration is running.
28 | Calibration will take approximately 2 seconds, you can view the raw output from calibration in console.
29 |
30 | Manual calibration offset tuning is also available (if you choose to use it), this is an advanced feature implemented for advanced users and developers. 31 | Standard users should stick to the automatic accelerometer calibration.
32 | You can view the calibration "effectivity" in 3D Vehicle tab.
33 |

34 |
35 |
36 |
37 |
38 | 39 | Update 40 |
41 |

42 | Minimum Armed throttle defines the minimum "speed" of the motors when you arm your flight controller.
43 | Default is 1100 (that means 10% throttle, 2000 = 100% throttle). 44 |

45 |
46 |
47 | Initialize EEPROM 48 |

49 | This step is mostly "optional" as Flight Controller automatically re-initializes the EEPROM after firmware upload.
50 | In case you encounter "weird" values/behaviour in the PID Tuning section, this is the button to press.
51 | Re-powering the Flight Controller afterwards is recommended.
52 |

53 |
54 |
55 |
56 |
57 | Backup 58 | Restore 59 |
60 |

61 | In case often re-flashing of the flight software (which in most cases wipes the EEPROM memory), you can backup 62 | and later restore most of the primary configuration values (PID, accel calibration, accel offset, channel mapping, etc) stored inside EEPROM memory. 63 |

64 |
65 |
66 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/initial_setup.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_initial_setup() { 2 | // Fill in the manual calibration values from eeprom 3 | var i = 0; 4 | $('#content .calibrateAccelManual input').each(function() { 5 | $(this).val(eepromConfig.ACCEL_BIAS[i++]); 6 | }); 7 | 8 | $('#content .minimumThrottle input').val(eepromConfig.minimumArmedThrottle); 9 | 10 | $('#content .calibrateESC').click(function() { 11 | eepromConfig.calibrateESC = parseInt(1); 12 | 13 | // Send updated UNION to the flight controller 14 | sendUNION(); 15 | }); 16 | 17 | $('#content .calibrateAccel').click(function() { 18 | // Start accel calibration 19 | command_log('Starting Accel calibration...'); 20 | send_message(PSP.PSP_SET_ACCEL_CALIBRATION, 1); 21 | }); 22 | 23 | $('#content .calibrateAccelManualUpdate').click(function() { 24 | // Update eeprom object 25 | var i = 0; 26 | $('.tab-initial_setup .calibrateAccelManual input').each(function() { 27 | eepromConfig.ACCEL_BIAS[i++] = parseInt($(this).val()); 28 | }); 29 | 30 | // Send updated UNION to the flight controller 31 | sendUNION(); 32 | }); 33 | 34 | $('#content .minimumThrottleUpdate').click(function() { 35 | // Update eeprom object 36 | eepromConfig.minimumArmedThrottle = parseInt($('#content .minimumThrottle input').val()); 37 | 38 | // Send updated UNION to the flight controller 39 | sendUNION(); 40 | }); 41 | 42 | $('#content .initializeEEPROM').click(function() { 43 | // initialize EEPROM 44 | command_log('Requesting EEPROM re-initialization.'); 45 | send_message(PSP.PSP_SET_EEPROM_REINIT, 1); 46 | }); 47 | 48 | $('#content .backup').click(configuration_backup); 49 | 50 | $('#content .restore').click(configuration_restore); 51 | } 52 | 53 | function process_accel_calibration() { 54 | command_log('Accel Calibration data received: ' + eepromConfig.ACCEL_BIAS); 55 | 56 | // Update the manual calibration inputs with latest data 57 | var i = 0; 58 | $('#content .calibrateAccelManual input').each(function() { 59 | $(this).val(eepromConfig.ACCEL_BIAS[i++]); 60 | }); 61 | 62 | $('#content .minimumThrottle input').val(eepromConfig.minimumArmedThrottle); 63 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/motor_output.html: -------------------------------------------------------------------------------- 1 |
2 | 12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 | 33 |
34 | 44 | STOP ALL 45 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/motor_output.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_motor_output() { 2 | // Request current state of motors 3 | send_message(PSP.PSP_REQ_MOTORS_OUTPUT, 1); 4 | 5 | // Enable specific titles 6 | for (var i = 0; i < motors; i++) { 7 | $('div.tab-motor_output .titles li:eq(' + i + ')').addClass('active'); 8 | } 9 | 10 | // Disable the rest 11 | $('ul.sliders input').each(function() { 12 | var index = $(this).parent().index(); 13 | 14 | if (index >= motors) { 15 | $(this).attr('disabled', 'disabled'); 16 | } 17 | }); 18 | 19 | $('.tab-motor_output .stop').click(function() { 20 | // reset all to 0 21 | $('ul.sliders input').each(function() { 22 | if ($(this).attr('disabled') != 'disabled') { // Protects .change event firing on motors that are disabled 23 | $(this).val(0); 24 | 25 | // trigger change events 26 | $(this).change(); 27 | } 28 | }); 29 | 30 | }); 31 | 32 | // Delayed initialize (due to current motor output requirements 33 | setTimeout(function() { 34 | for (var i = 0; i < motors; i++) { 35 | $('ul.sliders input').eq(i).val((motors_output[i] - 1000) / 10); 36 | $('ul.values li').eq(i).html(((motors_output[i] - 1000) / 10) + ' %'); 37 | } 38 | 39 | // Change handler is "hooked up" after all the data is processed (to avoid double sending of the values) 40 | $('ul.sliders input').change(function() { 41 | var motor_n = parseInt($(this).parent().index()); // motor number 42 | var motor_v = parseInt($(this).val()); // motor value 43 | 44 | // Update UI 45 | $('ul.values li').eq(motor_n).html(motor_v + ' %'); 46 | 47 | // Send data to flight controller 48 | send_message(PSP.PSP_SET_MOTOR_TEST_VALUE, [motor_n, motor_v]); 49 | }); 50 | 51 | // request motor out data from flight controller 52 | timers.push(setInterval(motor_pull, 50)); 53 | }, 25); 54 | } 55 | 56 | function motor_pull() { 57 | // Render data 58 | for (var i = 0; i < motors; i++) { 59 | motors_output[i] -= 1000; 60 | var margin_top = 130.0 - (motors_output[i] * 0.13); 61 | var height = (motors_output[i] * 0.13); 62 | var color = parseInt(motors_output[i] * 0.256); 63 | $('.motor-' + i + ' .indicator').css({'margin-top' : margin_top + 'px', 'height' : height + 'px', 'background-color' : 'rgb(' + color + ',0,0)'}); 64 | } 65 | 66 | // request new data 67 | send_message(PSP.PSP_REQ_MOTORS_OUTPUT, 1); 68 | } 69 | -------------------------------------------------------------------------------- /extras/Configurator/tabs/pid_tuning.html: -------------------------------------------------------------------------------- 1 |
2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 |
NameProportionalIntegralDerivativeWindUp GuardAction
Command YAWUpdate
Command PITCHUpdate
Command RollUpdate
YAWUpdate
PITCHUpdate
ROLLUpdate
BarometerUpdate
Sonar (range finder)Update
GPSUpdate
100 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/pid_tuning.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_pid_tuning() { 2 | var i = 0; 3 | 4 | // Command 5 | $('#content .command-yaw input').each(function() { 6 | if (i != 3) { 7 | $(this).val(eepromConfig.PID_YAW_c[i++].toFixed(2)); 8 | } else { 9 | $(this).val(eepromConfig.PID_YAW_c[i++].toFixed(0)); 10 | } 11 | }); 12 | 13 | i = 0; 14 | $('#content .command-pitch input').each(function() { 15 | if (i != 3) { 16 | $(this).val(eepromConfig.PID_PITCH_c[i++].toFixed(2)); 17 | } else { 18 | $(this).val(eepromConfig.PID_PITCH_c[i++].toFixed(0)); 19 | } 20 | }); 21 | 22 | i = 0; 23 | $('#content .command-roll input').each(function() { 24 | if (i != 3) { 25 | $(this).val(eepromConfig.PID_ROLL_c[i++].toFixed(2)); 26 | } else { 27 | $(this).val(eepromConfig.PID_ROLL_c[i++].toFixed(0)); 28 | } 29 | }); 30 | 31 | // Motor 32 | i = 0; 33 | $('#content .motor-yaw input').each(function() { 34 | if (i != 3) { 35 | $(this).val(eepromConfig.PID_YAW_m[i++].toFixed(2)); 36 | } else { 37 | $(this).val(eepromConfig.PID_YAW_m[i++].toFixed(0)); 38 | } 39 | }); 40 | 41 | i = 0; 42 | $('#content .motor-pitch input').each(function() { 43 | if (i != 3) { 44 | $(this).val(eepromConfig.PID_PITCH_m[i++].toFixed(2)); 45 | } else { 46 | $(this).val(eepromConfig.PID_PITCH_m[i++].toFixed(0)); 47 | } 48 | }); 49 | 50 | i = 0; 51 | $('#content .motor-roll input').each(function() { 52 | if (i != 3) { 53 | $(this).val(eepromConfig.PID_ROLL_m[i++].toFixed(2)); 54 | } else { 55 | $(this).val(eepromConfig.PID_ROLL_m[i++].toFixed(0)); 56 | } 57 | }); 58 | 59 | // Baro 60 | i = 0; 61 | $('#content .baro input').each(function() { 62 | if (i != 3) { 63 | $(this).val(eepromConfig.PID_BARO[i++].toFixed(2)); 64 | } else { 65 | $(this).val(eepromConfig.PID_BARO[i++].toFixed(0)); 66 | } 67 | }); 68 | 69 | // Sonar 70 | i = 0; 71 | $('#content .sonar input').each(function() { 72 | if (i != 3) { 73 | $(this).val(eepromConfig.PID_SONAR[i++].toFixed(2)); 74 | } else { 75 | $(this).val(eepromConfig.PID_SONAR[i++].toFixed(0)); 76 | } 77 | }); 78 | 79 | // GPS 80 | i = 0; 81 | $('#content .GPS input').each(function() { 82 | if (i != 3) { 83 | $(this).val(eepromConfig.PID_GPS[i++].toFixed(2)); 84 | } else { 85 | $(this).val(eepromConfig.PID_GPS[i++].toFixed(0)); 86 | } 87 | }); 88 | 89 | // UI hooks 90 | $('#content .pid_tuning a.update').click(function() { 91 | var parent = $(this).parent().parent(); 92 | 93 | var i = 0; 94 | switch (parent.index()) { 95 | case 1: // command yaw 96 | $('input', parent).each(function() { 97 | eepromConfig.PID_YAW_c[i] = parseFloat($(this).val()); 98 | i++; 99 | }); 100 | break; 101 | case 2: // command pitch 102 | $('input', parent).each(function() { 103 | eepromConfig.PID_PITCH_c[i] = parseFloat($(this).val()); 104 | i++; 105 | }); 106 | break; 107 | case 3: // command roll 108 | $('input', parent).each(function() { 109 | eepromConfig.PID_ROLL_c[i] = parseFloat($(this).val()); 110 | i++; 111 | }); 112 | break; 113 | case 5: // yaw 114 | $('input', parent).each(function() { 115 | eepromConfig.PID_YAW_m[i] = parseFloat($(this).val()); 116 | i++; 117 | }); 118 | break; 119 | case 6: // pitch 120 | $('input', parent).each(function() { 121 | eepromConfig.PID_PITCH_m[i] = parseFloat($(this).val()); 122 | i++; 123 | }); 124 | break; 125 | case 7: // roll 126 | $('input', parent).each(function() { 127 | eepromConfig.PID_ROLL_m[i] = parseFloat($(this).val()); 128 | i++; 129 | }); 130 | break; 131 | case 9: // baro 132 | $('input', parent).each(function() { 133 | eepromConfig.PID_BARO[i] = parseFloat($(this).val()); 134 | i++; 135 | }); 136 | break; 137 | case 10: // sonar 138 | $('input', parent).each(function() { 139 | eepromConfig.PID_SONAR[i] = parseFloat($(this).val()); 140 | i++; 141 | }); 142 | break; 143 | case 11: // GPS 144 | $('input', parent).each(function() { 145 | eepromConfig.PID_GPS[i] = parseFloat($(this).val()); 146 | i++; 147 | }); 148 | break; 149 | } 150 | 151 | // Send updated UNION to the flight controller 152 | sendUNION(); 153 | }); 154 | }; -------------------------------------------------------------------------------- /extras/Configurator/tabs/rx.html: -------------------------------------------------------------------------------- 1 |
2 |
3 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/rx.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_rx() { 2 | // Setup variables 3 | samples_i = 300; 4 | 5 | for (var i = 0; i < receiver_data.plot.length; i++) { 6 | receiver_data.plot[i] = []; 7 | } 8 | 9 | for (var i = 0; i <= 300; i++) { 10 | receiver_data.plot[0].push([i, 0]); 11 | receiver_data.plot[1].push([i, 0]); 12 | receiver_data.plot[2].push([i, 0]); 13 | receiver_data.plot[3].push([i, 0]); 14 | receiver_data.plot[4].push([i, 0]); 15 | receiver_data.plot[5].push([i, 0]); 16 | receiver_data.plot[6].push([i, 0]); 17 | receiver_data.plot[7].push([i, 0]); 18 | receiver_data.plot[8].push([i, 0]); 19 | receiver_data.plot[9].push([i, 0]); 20 | receiver_data.plot[10].push([i, 0]); 21 | receiver_data.plot[11].push([i, 0]); 22 | receiver_data.plot[12].push([i, 0]); 23 | receiver_data.plot[13].push([i, 0]); 24 | receiver_data.plot[14].push([i, 0]); 25 | receiver_data.plot[15].push([i, 0]); 26 | } 27 | 28 | // Graph definitions 29 | e_graph_receiver = document.getElementById("graph_receiver"); 30 | 31 | receiver_options = { 32 | shadowSize: 0, 33 | yaxis : { 34 | max: 2200, 35 | min: 800 36 | }, 37 | xaxis : { 38 | }, 39 | grid : { 40 | backgroundColor: "#FFFFFF" 41 | }, 42 | legend : { 43 | backgroundOpacity: 0 44 | } 45 | } 46 | 47 | // request receiver data from flight controller 48 | timers.push(setInterval(rx_poll, 50)); 49 | } 50 | 51 | function RX_channel_name(num) { 52 | var name; 53 | 54 | switch (num) { 55 | case 0: 56 | name = 'ROLL'; 57 | break; 58 | case 1: 59 | name = 'PITCH'; 60 | break; 61 | case 2: 62 | name = 'THROTTLE'; 63 | break; 64 | case 3: 65 | name = 'YAW/RUDDER'; 66 | break; 67 | case 4: 68 | name = 'AUX 1'; 69 | break; 70 | case 5: 71 | name = 'AUX 2'; 72 | break; 73 | case 6: 74 | name = 'AUX 3'; 75 | break; 76 | case 7: 77 | name = 'AUX 4'; 78 | break; 79 | 80 | case 8: 81 | name = 'AUX 5'; 82 | break; 83 | case 9: 84 | name = 'AUX 6'; 85 | break; 86 | case 10: 87 | name = 'AUX 7'; 88 | break; 89 | case 11: 90 | name = 'AUX 8'; 91 | break; 92 | case 12: 93 | name = 'AUX 9'; 94 | break; 95 | case 13: 96 | name = 'AUX 10'; 97 | break; 98 | case 14: 99 | name = 'AUX 11'; 100 | break; 101 | case 15: 102 | name = 'AUX 12'; 103 | break; 104 | } 105 | 106 | return name; 107 | } 108 | 109 | function rx_poll() { 110 | // push latest data to the main array 111 | receiver_data.plot[0].push([samples_i, receiver_data.raw[0]]); 112 | receiver_data.plot[1].push([samples_i, receiver_data.raw[1]]); 113 | receiver_data.plot[2].push([samples_i, receiver_data.raw[2]]); 114 | receiver_data.plot[3].push([samples_i, receiver_data.raw[3]]); 115 | receiver_data.plot[4].push([samples_i, receiver_data.raw[4]]); 116 | receiver_data.plot[5].push([samples_i, receiver_data.raw[5]]); 117 | receiver_data.plot[6].push([samples_i, receiver_data.raw[6]]); 118 | receiver_data.plot[7].push([samples_i, receiver_data.raw[7]]); 119 | receiver_data.plot[8].push([samples_i, receiver_data.raw[8]]); 120 | receiver_data.plot[9].push([samples_i, receiver_data.raw[9]]); 121 | receiver_data.plot[10].push([samples_i, receiver_data.raw[10]]); 122 | receiver_data.plot[11].push([samples_i, receiver_data.raw[11]]); 123 | receiver_data.plot[12].push([samples_i, receiver_data.raw[12]]); 124 | receiver_data.plot[13].push([samples_i, receiver_data.raw[13]]); 125 | receiver_data.plot[14].push([samples_i, receiver_data.raw[14]]); 126 | receiver_data.plot[15].push([samples_i, receiver_data.raw[15]]); 127 | 128 | // Remove old data from array 129 | while (receiver_data.plot[0].length > 300) { 130 | receiver_data.plot[0].shift(); 131 | receiver_data.plot[1].shift(); 132 | receiver_data.plot[2].shift(); 133 | receiver_data.plot[3].shift(); 134 | receiver_data.plot[4].shift(); 135 | receiver_data.plot[5].shift(); 136 | receiver_data.plot[6].shift(); 137 | receiver_data.plot[7].shift(); 138 | receiver_data.plot[8].shift(); 139 | receiver_data.plot[9].shift(); 140 | receiver_data.plot[10].shift(); 141 | receiver_data.plot[11].shift(); 142 | receiver_data.plot[12].shift(); 143 | receiver_data.plot[13].shift(); 144 | receiver_data.plot[14].shift(); 145 | receiver_data.plot[15].shift(); 146 | } 147 | 148 | Flotr.draw(e_graph_receiver, [ 149 | {data: receiver_data.plot[0], label: "CH-0 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[0]) + " [" + receiver_data.raw[0] + "]"}, 150 | {data: receiver_data.plot[1], label: "CH-1 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[1]) + " [" + receiver_data.raw[1] + "]"}, 151 | {data: receiver_data.plot[2], label: "CH-2 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[2]) + " [" + receiver_data.raw[2] + "]"}, 152 | {data: receiver_data.plot[3], label: "CH-3 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[3]) + " [" + receiver_data.raw[3] + "]"}, 153 | {data: receiver_data.plot[4], label: "CH-4 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[4]) + " [" + receiver_data.raw[4] + "]"}, 154 | {data: receiver_data.plot[5], label: "CH-5 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[5]) + " [" + receiver_data.raw[5] + "]"}, 155 | {data: receiver_data.plot[6], label: "CH-6 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[6]) + " [" + receiver_data.raw[6] + "]"}, 156 | {data: receiver_data.plot[7], label: "CH-7 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[7]) + " [" + receiver_data.raw[7] + "]"}, 157 | {data: receiver_data.plot[8], label: "CH-8 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[8]) + " [" + receiver_data.raw[8] + "]"}, 158 | {data: receiver_data.plot[9], label: "CH-9 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[9]) + " [" + receiver_data.raw[9] + "]"}, 159 | {data: receiver_data.plot[10], label: "CH-10 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[10]) + " [" + receiver_data.raw[10] + "]"}, 160 | {data: receiver_data.plot[11], label: "CH-11 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[11]) + " [" + receiver_data.raw[11] + "]"}, 161 | {data: receiver_data.plot[12], label: "CH-12 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[12]) + " [" + receiver_data.raw[12] + "]"}, 162 | {data: receiver_data.plot[13], label: "CH-13 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[13]) + " [" + receiver_data.raw[13] + "]"}, 163 | {data: receiver_data.plot[14], label: "CH-14 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[14]) + " [" + receiver_data.raw[14] + "]"}, 164 | {data: receiver_data.plot[15], label: "CH-15 - " + RX_channel_name(eepromConfig.CHANNEL_ASSIGNMENT[15]) + " [" + receiver_data.raw[15] + "]"} ], receiver_options); 165 | 166 | samples_i++; 167 | 168 | // request new data 169 | send_message(PSP.PSP_REQ_RC, 1); 170 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/sensor_data.html: -------------------------------------------------------------------------------- 1 |
2 |
3 |
4 |
5 |
6 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/sensor_data.js: -------------------------------------------------------------------------------- 1 | function tab_initialize_sensor_data() { 2 | // Setup variables 3 | samples_i = 300; 4 | 5 | for (var i = 0; i < sensor_data.gyro_plot.length; i++) { 6 | sensor_data.gyro_plot[i] = []; 7 | sensor_data.accel_plot[i] = []; 8 | sensor_data.mag_plot[i] = []; 9 | } 10 | sensor_data.baro_plot = []; 11 | 12 | for (var i = 0; i <= 300; i++) { 13 | sensor_data.gyro_plot[0].push([i, 0]); 14 | sensor_data.gyro_plot[1].push([i, 0]); 15 | sensor_data.gyro_plot[2].push([i, 0]); 16 | 17 | sensor_data.accel_plot[0].push([i, 0]); 18 | sensor_data.accel_plot[1].push([i, 0]); 19 | sensor_data.accel_plot[2].push([i, 0]); 20 | 21 | sensor_data.mag_plot[0].push([i, 0]); 22 | sensor_data.mag_plot[1].push([i, 0]); 23 | sensor_data.mag_plot[2].push([i, 0]); 24 | 25 | sensor_data.baro_plot.push([i, 0]); 26 | } 27 | 28 | // Graph definitions 29 | e_graph_gyro = document.getElementById("graph_gyro"); 30 | e_graph_accel = document.getElementById("graph_accel"); 31 | e_graph_mag = document.getElementById("graph_mag"); 32 | e_graph_baro = document.getElementById("graph_baro"); 33 | 34 | gyro_options = { 35 | title: "Gyroscope (deg/s)", 36 | shadowSize: 0, 37 | yaxis: { 38 | max: 10, 39 | min: -10 40 | }, 41 | xaxis: { 42 | //noTicks = 0 43 | }, 44 | grid: { 45 | backgroundColor: "#FFFFFF" 46 | }, 47 | legend: { 48 | position: "wn", 49 | backgroundOpacity: 0 50 | } 51 | } 52 | 53 | accel_options = { 54 | title: "Accelerometer (g)", 55 | shadowSize: 0, 56 | yaxis: { 57 | max : 1.5, 58 | min : -1.5 59 | }, 60 | xaxis: { 61 | }, 62 | grid: { 63 | backgroundColor : "#FFFFFF" 64 | }, 65 | legend: { 66 | position: "wn", 67 | backgroundOpacity: 0 68 | } 69 | } 70 | 71 | mag_options = { 72 | title: "Magnetometer (Ga)", 73 | shadowSize: 0, 74 | yaxis: { 75 | }, 76 | xaxis: { 77 | }, 78 | grid: { 79 | backgroundColor : "#FFFFFF" 80 | }, 81 | legend: { 82 | position: "wn", 83 | backgroundOpacity: 0 84 | } 85 | } 86 | 87 | baro_options = { 88 | title: "Barometer (meters above sea level)", 89 | shadowSize: 0, 90 | yaxis: { 91 | tickDecimals: 0, 92 | }, 93 | xaxis: { 94 | }, 95 | grid: { 96 | backgroundColor : "#FFFFFF" 97 | }, 98 | legend: { 99 | position: "wn", 100 | backgroundOpacity: 0 101 | } 102 | } 103 | 104 | // disable plots for sensors that wasn't detected 105 | if (sensors_alive.gyro == 0) { 106 | $(e_graph_gyro).hide(); 107 | } 108 | 109 | if (sensors_alive.accel == 0) { 110 | $(e_graph_accel).hide(); 111 | } 112 | 113 | if (sensors_alive.mag == 0) { 114 | $(e_graph_mag).hide(); 115 | } 116 | 117 | if (sensors_alive.baro == 0) { 118 | $(e_graph_baro).hide(); 119 | } 120 | 121 | // request sensor data from flight controller 122 | timers.push(setInterval(sensor_pull, 50)); 123 | } 124 | 125 | function sensor_pull() { 126 | if (sensors_alive.gyro == 1 || sensors_alive.accel == 1) { 127 | // push latest data to the main array 128 | sensor_data.gyro_plot[0].push([samples_i, sensor_data.gyro[0]]); 129 | sensor_data.gyro_plot[1].push([samples_i, sensor_data.gyro[1]]); 130 | sensor_data.gyro_plot[2].push([samples_i, sensor_data.gyro[2]]); 131 | 132 | sensor_data.accel_plot[0].push([samples_i, sensor_data.accel[0]]); 133 | sensor_data.accel_plot[1].push([samples_i, sensor_data.accel[1]]); 134 | sensor_data.accel_plot[2].push([samples_i, sensor_data.accel[2]]); 135 | 136 | // Remove old data from array 137 | while (sensor_data.gyro_plot[0].length > 300) { 138 | sensor_data.gyro_plot[0].shift(); 139 | sensor_data.gyro_plot[1].shift(); 140 | sensor_data.gyro_plot[2].shift(); 141 | 142 | sensor_data.accel_plot[0].shift(); 143 | sensor_data.accel_plot[1].shift(); 144 | sensor_data.accel_plot[2].shift(); 145 | }; 146 | 147 | // Update graphs 148 | Flotr.draw(e_graph_gyro, [ 149 | {data: sensor_data.gyro_plot[0], label: "X - rate [" + sensor_data.gyro[0].toFixed(2) + "]"}, 150 | {data: sensor_data.gyro_plot[1], label: "Y - rate [" + sensor_data.gyro[1].toFixed(2) + "]"}, 151 | {data: sensor_data.gyro_plot[2], label: "Z - rate [" + sensor_data.gyro[2].toFixed(2) + "]"} ], gyro_options); 152 | 153 | Flotr.draw(e_graph_accel, [ 154 | {data: sensor_data.accel_plot[0], label: "X - acceleration [" + sensor_data.accel[0].toFixed(2) + "]"}, 155 | {data: sensor_data.accel_plot[1], label: "Y - acceleration [" + sensor_data.accel[1].toFixed(2) + "]"}, 156 | {data: sensor_data.accel_plot[2], label: "Z - acceleration [" + sensor_data.accel[2].toFixed(2) + "]"} ], accel_options); 157 | 158 | // request new data 159 | send_message(PSP.PSP_REQ_GYRO_ACC, 1); 160 | } 161 | 162 | if (sensors_alive.mag == 1) { 163 | // push latest data to the main array 164 | sensor_data.mag_plot[0].push([samples_i, sensor_data.mag[0]]); 165 | sensor_data.mag_plot[1].push([samples_i, sensor_data.mag[1]]); 166 | sensor_data.mag_plot[2].push([samples_i, sensor_data.mag[2]]); 167 | 168 | // Remove old data from array 169 | while (sensor_data.mag_plot[0].length > 300) { 170 | sensor_data.mag_plot[0].shift(); 171 | sensor_data.mag_plot[1].shift(); 172 | sensor_data.mag_plot[2].shift(); 173 | } 174 | 175 | // Update graph 176 | Flotr.draw(e_graph_mag, [ 177 | {data: sensor_data.mag_plot[0], label: "X - Ga [" + sensor_data.mag[0].toFixed(2) + "]"}, 178 | {data: sensor_data.mag_plot[1], label: "Y - Ga [" + sensor_data.mag[1].toFixed(2) + "]"}, 179 | {data: sensor_data.mag_plot[2], label: "Z - Ga [" + sensor_data.mag[2].toFixed(2) + "]"} ], mag_options); 180 | 181 | // request new data 182 | send_message(PSP.PSP_REQ_MAG, 1); 183 | } 184 | 185 | if (sensors_alive.baro == 1) { 186 | // push latest data to the main array 187 | sensor_data.baro_plot.push([samples_i, sensor_data.baro[1]]); 188 | 189 | // Remove old data from array 190 | while (sensor_data.baro_plot.length > 300) { 191 | sensor_data.baro_plot.shift(); 192 | } 193 | 194 | // Update graph 195 | Flotr.draw(e_graph_baro, [ 196 | {data: sensor_data.baro_plot, label: "X - Meters [" + sensor_data.baro[1].toFixed(2) + "]"} ], baro_options); 197 | 198 | // request new data 199 | send_message(PSP.PSP_REQ_BARO, 1); 200 | } 201 | 202 | samples_i++; 203 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/sensor_mapping.html: -------------------------------------------------------------------------------- 1 |
2 |
3 | Gyroscope 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 |
AxisXYZSign
1
2
3
34 |
35 |
36 | Accelerometer 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 |
AxisXYZSign
1
2
3
67 |
68 |
69 | Magnetometer 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 |
AxisXYZSign
1
2
3
100 |
101 | Restore Default 102 | Update 103 |
104 |

105 | Note: You can view the changes in real time inside the Sensor Data and Vehicle View tabs. 106 |

107 |

108 | Note: To complete the "restore to default" process, 109 | you need to restart / powercycle your flight controller. 110 |

111 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/sensor_mapping.js: -------------------------------------------------------------------------------- 1 | var TEMP_SENSOR_AXIS_MAP; 2 | 3 | function tab_initialize_sensor_mapping() { 4 | // populate the select boxes 5 | // gyroscope 6 | gyro_axis1 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[0], 0); 7 | gyro_axis2 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[0], 3); 8 | gyro_axis3 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[0], 6); 9 | 10 | $('table.gyroscope tr:eq(1) input').eq(gyro_axis1.axis).prop('checked', true); 11 | $('table.gyroscope tr:eq(1) input').eq(3).prop('checked', gyro_axis1.sign); 12 | $('table.gyroscope tr:eq(2) input').eq(gyro_axis2.axis).prop('checked', true); 13 | $('table.gyroscope tr:eq(2) input').eq(3).prop('checked', gyro_axis2.sign); 14 | $('table.gyroscope tr:eq(3) input').eq(gyro_axis3.axis).prop('checked', true); 15 | $('table.gyroscope tr:eq(3) input').eq(3).prop('checked', gyro_axis3.sign); 16 | 17 | // accelerometer 18 | accel_axis1 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[1], 0); 19 | accel_axis2 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[1], 3); 20 | accel_axis3 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[1], 6); 21 | 22 | $('table.accelerometer tr:eq(1) input').eq(accel_axis1.axis).prop('checked', true); 23 | $('table.accelerometer tr:eq(1) input').eq(3).prop('checked', accel_axis1.sign); 24 | $('table.accelerometer tr:eq(2) input').eq(accel_axis2.axis).prop('checked', true); 25 | $('table.accelerometer tr:eq(2) input').eq(3).prop('checked', accel_axis2.sign); 26 | $('table.accelerometer tr:eq(3) input').eq(accel_axis3.axis).prop('checked', true); 27 | $('table.accelerometer tr:eq(3) input').eq(3).prop('checked', accel_axis3.sign); 28 | 29 | // magnetometer 30 | mag_axis1 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[2], 0); 31 | mag_axis2 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[2], 3); 32 | mag_axis3 = check_axis_position(eepromConfig.SENSOR_AXIS_MAP[2], 6); 33 | 34 | $('table.magnetometer tr:eq(1) input').eq(mag_axis1.axis).prop('checked', true); 35 | $('table.magnetometer tr:eq(1) input').eq(3).prop('checked', mag_axis1.sign); 36 | $('table.magnetometer tr:eq(2) input').eq(mag_axis2.axis).prop('checked', true); 37 | $('table.magnetometer tr:eq(2) input').eq(3).prop('checked', mag_axis2.sign); 38 | $('table.magnetometer tr:eq(3) input').eq(mag_axis3.axis).prop('checked', true); 39 | $('table.magnetometer tr:eq(3) input').eq(3).prop('checked', mag_axis3.sign); 40 | 41 | 42 | // UI hooks 43 | $('table input').click(function() { 44 | parent = $(this).parent().parent(); 45 | 46 | if ($(this).attr('name') == 'sign') { 47 | // Special handling for sign 48 | } else { 49 | // Standard handling for axis 50 | $('input', parent).not('[name="sign"]').removeAttr('checked'); 51 | $(this).prop('checked', true); 52 | } 53 | }); 54 | 55 | $('div.tab-sensor_mapping a.restore').click(function() { 56 | eepromConfig.SENSOR_AXIS_MAP = [0, 0, 0]; 57 | 58 | // Send updated UNION to the flight controller 59 | sendUNION(); 60 | 61 | // refresh the UI 62 | $('#content').load("./tabs/sensor_mapping.html", tab_initialize_sensor_mapping); 63 | }); 64 | 65 | $('div.tab-sensor_mapping a.update').click(function() { 66 | // re-calculate temporary maps 67 | TEMP_SENSOR_AXIS_MAP = [0, 0, 0]; 68 | $('table.gyroscope tr:eq(1) input:checked').each(function(index, element) { 69 | set_axis_position(element, 0, 0); 70 | }); 71 | $('table.gyroscope tr:eq(2) input:checked').each(function(index, element) { 72 | set_axis_position(element, 0, 3); 73 | }); 74 | $('table.gyroscope tr:eq(3) input:checked').each(function(index, element) { 75 | set_axis_position(element, 0, 6); 76 | }); 77 | 78 | $('table.accelerometer tr:eq(1) input:checked').each(function(index, element) { 79 | set_axis_position(element, 1, 0); 80 | }); 81 | $('table.accelerometer tr:eq(2) input:checked').each(function(index, element) { 82 | set_axis_position(element, 1, 3); 83 | }); 84 | $('table.accelerometer tr:eq(3) input:checked').each(function(index, element) { 85 | set_axis_position(element, 1, 6); 86 | }); 87 | 88 | $('table.magnetometer tr:eq(1) input:checked').each(function(index, element) { 89 | set_axis_position(element, 2, 0); 90 | }); 91 | $('table.magnetometer tr:eq(2) input:checked').each(function(index, element) { 92 | set_axis_position(element, 2, 3); 93 | }); 94 | $('table.magnetometer tr:eq(3) input:checked').each(function(index, element) { 95 | set_axis_position(element, 2, 6); 96 | }); 97 | 98 | // we cant forget the initialization bit 99 | if (bit_check(eepromConfig.SENSOR_AXIS_MAP[0], 9)) { 100 | TEMP_SENSOR_AXIS_MAP[0] = bit_set(TEMP_SENSOR_AXIS_MAP[0], 9); 101 | } 102 | 103 | if (bit_check(eepromConfig.SENSOR_AXIS_MAP[1], 9)) { 104 | TEMP_SENSOR_AXIS_MAP[1] = bit_set(TEMP_SENSOR_AXIS_MAP[1], 9); 105 | } 106 | 107 | if (bit_check(eepromConfig.SENSOR_AXIS_MAP[2], 9)) { 108 | TEMP_SENSOR_AXIS_MAP[2] = bit_set(TEMP_SENSOR_AXIS_MAP[2], 9); 109 | } 110 | 111 | // push temp map values to the primary map 112 | eepromConfig.SENSOR_AXIS_MAP[0] = TEMP_SENSOR_AXIS_MAP[0]; 113 | eepromConfig.SENSOR_AXIS_MAP[1] = TEMP_SENSOR_AXIS_MAP[1]; 114 | eepromConfig.SENSOR_AXIS_MAP[2] = TEMP_SENSOR_AXIS_MAP[2]; 115 | 116 | 117 | // Send updated UNION to the flight controller 118 | sendUNION(); 119 | }); 120 | } 121 | 122 | function check_axis_position(input, num) { 123 | var axis = 0; 124 | var sign = 0; 125 | 126 | if (bit_check(input, num)) { // offset start 127 | axis += 1; 128 | } 129 | 130 | if (bit_check(input, num + 1)) { // offset + 1 131 | axis += 2; 132 | } 133 | 134 | if (bit_check(input, num + 2)) { // offset + 2 135 | sign += 1; 136 | } 137 | 138 | return {axis: axis, sign: sign}; 139 | } 140 | 141 | function set_axis_position(element, sensor, offset) { 142 | var name = $(element).attr('name'); 143 | 144 | switch(name) { 145 | case 'x': 146 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_clear(TEMP_SENSOR_AXIS_MAP[sensor], offset); 147 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_clear(TEMP_SENSOR_AXIS_MAP[sensor], offset + 1); 148 | break; 149 | case 'y': 150 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_set(TEMP_SENSOR_AXIS_MAP[sensor], offset); 151 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_clear(TEMP_SENSOR_AXIS_MAP[sensor], offset + 1); 152 | break; 153 | case 'z': 154 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_clear(TEMP_SENSOR_AXIS_MAP[sensor], offset); 155 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_set(TEMP_SENSOR_AXIS_MAP[sensor], offset + 1); 156 | break; 157 | case 'sign': 158 | TEMP_SENSOR_AXIS_MAP[sensor] = bit_set(TEMP_SENSOR_AXIS_MAP[sensor], offset + 2); 159 | break; 160 | } 161 | } -------------------------------------------------------------------------------- /extras/Configurator/tabs/vehicle_view.html: -------------------------------------------------------------------------------- 1 |
2 |
3 | Reset Z axis 4 |
5 |
6 |
7 |
8 |
BOTTOM
9 |
BACK
10 |
RIGHT
11 |
FRONT
12 |
LEFT
13 |
TOP
14 |
15 |
16 |
17 |
18 |
19 |
-------------------------------------------------------------------------------- /extras/Configurator/tabs/vehicle_view.js: -------------------------------------------------------------------------------- 1 | var yaw_fix = 0.0; 2 | 3 | function tab_initialize_vehicle_view() { 4 | // reset yaw button hook 5 | $('div#interactive_block > a.reset').click(function() { 6 | yaw_fix = sensor_data.kinematics[2]; 7 | console.log("YAW reset to 0"); 8 | }); 9 | 10 | // request kinematics data from flight controller 11 | timers.push(setInterval(kinematics_pull, 50)); 12 | } 13 | 14 | function kinematics_pull() { 15 | // update cube with latest data 16 | var cube = $('div#cube'); 17 | 18 | cube.css('-webkit-transform', 'rotateY(' + (sensor_data.kinematics[2] - yaw_fix) + 'deg)'); 19 | $('#cubePITCH', cube).css('-webkit-transform', 'rotateX(' + sensor_data.kinematics[1] + 'deg)'); 20 | $('#cubeROLL', cube).css('-webkit-transform', 'rotateZ(' + sensor_data.kinematics[0] + 'deg)'); 21 | 22 | // request new data 23 | send_message(PSP.PSP_REQ_KINEMATICS, 1); 24 | } -------------------------------------------------------------------------------- /extras/Filters/MAF.h: -------------------------------------------------------------------------------- 1 | /* Moving Average Filter implementation 2 | Using ring buffer to store/swap between the received data. 3 | SmoothFactor (parsed as int to the constructor) can never be higher then 4 | MAF_ARRAYSIZE constant. 5 | */ 6 | 7 | #define MAF_ARRAYSIZE 20 8 | 9 | class MAF { 10 | public: 11 | // Constructor 12 | MAF(uint8_t Size) { 13 | // Save array size 14 | smoothFactor = Size; 15 | 16 | // Bear in mind that data[N] array is defined in private 17 | // but is not initialized. 18 | // For some reason the implementation works, but in case you encounter 19 | // "Weird behaviour", this is the place to look. 20 | 21 | // Initialize head 22 | head = 0; 23 | }; 24 | 25 | double update(double value) { 26 | // Store new value inside the array 27 | data[head] = value; 28 | head++; 29 | 30 | // If we reached end of the array, return to beginning 31 | if (head == smoothFactor) head = 0; 32 | 33 | double sum; 34 | for (uint8_t i = 0; i < smoothFactor; i++) { 35 | sum += data[i]; 36 | } 37 | 38 | sum /= smoothFactor; 39 | return sum; 40 | }; 41 | 42 | private: 43 | uint8_t smoothFactor; 44 | double data[MAF_ARRAYSIZE]; 45 | uint8_t head; 46 | }; -------------------------------------------------------------------------------- /extras/I2Cscanner/I2Cscanner.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint8_t address; 4 | int8_t status; 5 | 6 | void setup() { 7 | Serial.begin(115200); 8 | Wire.begin(); 9 | } 10 | 11 | void loop() { 12 | Serial.println("Scanning..."); 13 | 14 | for (address = 1; address <= 127; address++) { 15 | Wire.beginTransmission(address); 16 | status = Wire.endTransmission(); 17 | 18 | if (status == 0) { 19 | Serial.print("I2C device found at 0x"); 20 | Serial.println(address, HEX); 21 | } 22 | delay(5); 23 | } 24 | 25 | Serial.println("done"); 26 | 27 | delay(5000); 28 | } -------------------------------------------------------------------------------- /extras/PCB/Phoenix_shield_v0.1.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cTn-dev/Phoenix-FlightController/a7dbea2961efc2a3a4fe270170acadd7e617034b/extras/PCB/Phoenix_shield_v0.1.zip -------------------------------------------------------------------------------- /libraries/P_Accelerometer/ADXL345.h: -------------------------------------------------------------------------------- 1 | /* ADXL345 - (ADXL345 is on separate breakout) 2 | 3 | Beware ! ADXL345 sends the data in reverse (LOW, HIGH) 4 | 5 | This library requires some more documentation 6 | */ 7 | 8 | #define ADXL345_ADDRESS 0x53 9 | #define ADXL345_DEVID 0xE5 10 | 11 | float accel[3]; 12 | 13 | class ADXL345 { 14 | public: 15 | // Constructor 16 | ADXL345() { 17 | accelScaleFactor = 9.81 / 256.0; 18 | accelSamples = 0; 19 | }; 20 | 21 | void initialize() { 22 | // Check if sensor is alive 23 | Wire.beginTransmission(ADXL345_ADDRESS); 24 | Wire.write(0x00); 25 | Wire.endTransmission(); 26 | 27 | Wire.requestFrom(ADXL345_ADDRESS, 1); 28 | 29 | uint8_t register_value = Wire.read(); 30 | 31 | if (register_value == ADXL345_DEVID) { 32 | sensors.sensors_detected |= ACCELEROMETER_DETECTED; 33 | } else { 34 | return; 35 | } 36 | 37 | sensors.i2c_write8(ADXL345_ADDRESS, 0x2D, 0x08); // set device to *measure* 38 | sensors.i2c_write8(ADXL345_ADDRESS, 0x31, 0x09); // set full range and +/- 4G 39 | sensors.i2c_write8(ADXL345_ADDRESS, 0x2C, 0x0B); // 200hz sampling 40 | 41 | // setup axis mapping 42 | if (CONFIG.data.ACCEL_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 43 | CONFIG.data.ACCEL_AXIS_MAP.axis1 = 0; // x 44 | CONFIG.data.ACCEL_AXIS_MAP.axis2 = 1; // y 45 | CONFIG.data.ACCEL_AXIS_MAP.axis3 = 2; // z 46 | 47 | CONFIG.data.ACCEL_AXIS_MAP.axis1_sign = 0; 48 | CONFIG.data.ACCEL_AXIS_MAP.axis2_sign = 0; 49 | CONFIG.data.ACCEL_AXIS_MAP.axis3_sign = 0; 50 | 51 | CONFIG.data.ACCEL_AXIS_MAP.initialized = 1; 52 | 53 | // save default in eeprom 54 | writeEEPROM(); 55 | } 56 | }; 57 | 58 | // ~1280ms (only runs when requested) 59 | void calibrate_accel() { 60 | uint8_t i, count = 128; 61 | int32_t xSum = 0, ySum = 0, zSum = 0; 62 | 63 | for(i = 0; i < count; i++) { 64 | readAccelRaw(); 65 | xSum += accelRaw[XAXIS]; 66 | ySum += accelRaw[YAXIS]; 67 | zSum += accelRaw[ZAXIS]; 68 | delay(10); 69 | } 70 | 71 | CONFIG.data.ACCEL_BIAS[XAXIS] = xSum / count; 72 | CONFIG.data.ACCEL_BIAS[YAXIS] = ySum / count; 73 | CONFIG.data.ACCEL_BIAS[ZAXIS] = (zSum / count) - 256; // - 1G 74 | 75 | // Reverse calibration forces 76 | CONFIG.data.ACCEL_BIAS[XAXIS] *= -1; 77 | CONFIG.data.ACCEL_BIAS[YAXIS] *= -1; 78 | CONFIG.data.ACCEL_BIAS[ZAXIS] *= -1; 79 | }; 80 | 81 | void readAccelRaw() { 82 | Wire.beginTransmission(ADXL345_ADDRESS); 83 | Wire.write(0x32); 84 | Wire.endTransmission(); 85 | 86 | Wire.requestFrom(ADXL345_ADDRESS, 6); 87 | 88 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis1] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis1_sign?-1:1); 89 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis2] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis2_sign?-1:1); 90 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis3] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis3_sign?-1:1); 91 | }; 92 | 93 | void readAccelSum() { 94 | readAccelRaw(); 95 | 96 | accelSum[XAXIS] += accelRaw[XAXIS]; 97 | accelSum[YAXIS] += accelRaw[YAXIS]; 98 | accelSum[ZAXIS] += accelRaw[ZAXIS]; 99 | 100 | accelSamples++; 101 | }; 102 | 103 | void evaluateAccel() { 104 | // Calculate average 105 | accel[XAXIS] = accelSum[XAXIS] / accelSamples; 106 | accel[YAXIS] = accelSum[YAXIS] / accelSamples; 107 | accel[ZAXIS] = accelSum[ZAXIS] / accelSamples; 108 | 109 | // Apply offsets 110 | accel[XAXIS] += CONFIG.data.ACCEL_BIAS[XAXIS]; 111 | accel[YAXIS] += CONFIG.data.ACCEL_BIAS[YAXIS]; 112 | accel[ZAXIS] += CONFIG.data.ACCEL_BIAS[ZAXIS]; 113 | 114 | // Apply correct scaling (at this point accel reprensents +- 1g = 9.81 m/s^2) 115 | accel[XAXIS] *= accelScaleFactor; 116 | accel[YAXIS] *= accelScaleFactor; 117 | accel[ZAXIS] *= accelScaleFactor; 118 | 119 | // Reset SUM variables 120 | accelSum[XAXIS] = 0; 121 | accelSum[YAXIS] = 0; 122 | accelSum[ZAXIS] = 0; 123 | accelSamples = 0; 124 | }; 125 | 126 | private: 127 | float accelScaleFactor; 128 | 129 | int16_t accelRaw[3]; 130 | float accelSum[3]; 131 | 132 | uint8_t accelSamples; 133 | }; 134 | 135 | ADXL345 adxl; 136 | 137 | void SensorArray::initializeAccel() { 138 | adxl.initialize(); 139 | } 140 | 141 | void SensorArray::calibrateAccel() { 142 | adxl.calibrate_accel(); 143 | } 144 | 145 | void SensorArray::readAccelSum() { 146 | adxl.readAccelSum(); 147 | } 148 | 149 | void SensorArray::evaluateAccel() { 150 | adxl.evaluateAccel(); 151 | } 152 | -------------------------------------------------------------------------------- /libraries/P_Accelerometer/ADXL345_AQ_v21.h: -------------------------------------------------------------------------------- 1 | /* ADXL345, 9 DOF stick library 2 | 3 | Beware ! ADXL345 sends the data in reverse (LOW, HIGH) 4 | 5 | This library requires some more documentation 6 | (code was tested and works perfectly fine) 7 | */ 8 | 9 | #define ADXL345_ADDRESS 0x53 10 | #define ADXL345_DEVID 0xE5 11 | 12 | float accel[3]; 13 | 14 | class ADXL345 { 15 | public: 16 | // Constructor 17 | ADXL345() { 18 | accelScaleFactor = 9.81 / 256.0; 19 | accelSamples = 0; 20 | }; 21 | 22 | void initialize() { 23 | // Check if sensor is alive 24 | Wire.beginTransmission(ADXL345_ADDRESS); 25 | Wire.write(0x00); 26 | Wire.endTransmission(); 27 | 28 | Wire.requestFrom(ADXL345_ADDRESS, 1); 29 | 30 | uint8_t register_value = Wire.read(); 31 | 32 | if (register_value == ADXL345_DEVID) { 33 | sensors.sensors_detected |= ACCELEROMETER_DETECTED; 34 | } else { 35 | return; 36 | } 37 | 38 | sensors.i2c_write8(ADXL345_ADDRESS, 0x2D, 0x08); // set device to *measure* 39 | sensors.i2c_write8(ADXL345_ADDRESS, 0x31, 0x09); // set full range and +/- 4G 40 | sensors.i2c_write8(ADXL345_ADDRESS, 0x2C, 0x0B); // 200hz sampling 41 | 42 | // setup axis mapping 43 | if (CONFIG.data.ACCEL_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 44 | CONFIG.data.ACCEL_AXIS_MAP.axis1 = 1; // y 45 | CONFIG.data.ACCEL_AXIS_MAP.axis2 = 0; // x 46 | CONFIG.data.ACCEL_AXIS_MAP.axis3 = 2; // z 47 | 48 | CONFIG.data.ACCEL_AXIS_MAP.axis1_sign = 0; 49 | CONFIG.data.ACCEL_AXIS_MAP.axis2_sign = 0; 50 | CONFIG.data.ACCEL_AXIS_MAP.axis3_sign = 0; 51 | 52 | CONFIG.data.ACCEL_AXIS_MAP.initialized = 1; 53 | 54 | // save default in eeprom 55 | writeEEPROM(); 56 | } 57 | }; 58 | 59 | // ~1280ms (only runs when requested) 60 | void calibrate_accel() { 61 | uint8_t i, count = 128; 62 | int32_t xSum = 0, ySum = 0, zSum = 0; 63 | 64 | for(i = 0; i < count; i++) { 65 | readAccelRaw(); 66 | xSum += accelRaw[XAXIS]; 67 | ySum += accelRaw[YAXIS]; 68 | zSum += accelRaw[ZAXIS]; 69 | delay(10); 70 | } 71 | 72 | CONFIG.data.ACCEL_BIAS[XAXIS] = xSum / count; 73 | CONFIG.data.ACCEL_BIAS[YAXIS] = ySum / count; 74 | CONFIG.data.ACCEL_BIAS[ZAXIS] = (zSum / count) - 256; // - 1G 75 | 76 | // Reverse calibration forces 77 | CONFIG.data.ACCEL_BIAS[XAXIS] *= -1; 78 | CONFIG.data.ACCEL_BIAS[YAXIS] *= -1; 79 | CONFIG.data.ACCEL_BIAS[ZAXIS] *= -1; 80 | }; 81 | 82 | void readAccelRaw() { 83 | Wire.beginTransmission(ADXL345_ADDRESS); 84 | Wire.write(0x32); 85 | Wire.endTransmission(); 86 | 87 | Wire.requestFrom(ADXL345_ADDRESS, 6); 88 | 89 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis1] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis1_sign?-1:1); 90 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis2] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis2_sign?-1:1); 91 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis3] = (Wire.read() | (Wire.read() << 8)) * (CONFIG.data.ACCEL_AXIS_MAP.axis3_sign?-1:1); 92 | }; 93 | 94 | void readAccelSum() { 95 | readAccelRaw(); 96 | 97 | accelSum[XAXIS] += accelRaw[XAXIS]; 98 | accelSum[YAXIS] += accelRaw[YAXIS]; 99 | accelSum[ZAXIS] += accelRaw[ZAXIS]; 100 | 101 | accelSamples++; 102 | }; 103 | 104 | void evaluateAccel() { 105 | // Calculate average 106 | accel[XAXIS] = accelSum[XAXIS] / accelSamples; 107 | accel[YAXIS] = -(accelSum[YAXIS] / accelSamples); 108 | accel[ZAXIS] = accelSum[ZAXIS] / accelSamples; 109 | 110 | // Apply offsets 111 | accel[XAXIS] += CONFIG.data.ACCEL_BIAS[XAXIS]; 112 | accel[YAXIS] += CONFIG.data.ACCEL_BIAS[YAXIS]; 113 | accel[ZAXIS] += CONFIG.data.ACCEL_BIAS[ZAXIS]; 114 | 115 | // Apply correct scaling (at this point accel reprensents +- 1g = 9.81 m/s^2) 116 | accel[XAXIS] *= accelScaleFactor; 117 | accel[YAXIS] *= accelScaleFactor; 118 | accel[ZAXIS] *= accelScaleFactor; 119 | 120 | // Reset SUM variables 121 | accelSum[XAXIS] = 0; 122 | accelSum[YAXIS] = 0; 123 | accelSum[ZAXIS] = 0; 124 | accelSamples = 0; 125 | }; 126 | 127 | private: 128 | float accelScaleFactor; 129 | 130 | int16_t accelRaw[3]; 131 | float accelSum[3]; 132 | 133 | uint8_t accelSamples; 134 | }; 135 | 136 | ADXL345 adxl; 137 | 138 | void SensorArray::initializeAccel() { 139 | adxl.initialize(); 140 | } 141 | 142 | void SensorArray::calibrateAccel() { 143 | adxl.calibrate_accel(); 144 | } 145 | 146 | void SensorArray::readAccelSum() { 147 | adxl.readAccelSum(); 148 | } 149 | 150 | void SensorArray::evaluateAccel() { 151 | adxl.evaluateAccel(); 152 | } 153 | -------------------------------------------------------------------------------- /libraries/P_Accelerometer/BMA180_AQ_v20.h: -------------------------------------------------------------------------------- 1 | /* BMA180 library 2 | 3 | Beware ! BMA180 sends the data in reverse (LOW, HIGH) 4 | 5 | Big thanks to remspoor on #aeroquad IRC for testing this integration 6 | 7 | This library requires some more documentation 8 | */ 9 | 10 | #define BMA180_ADDRESS 0x40 11 | 12 | #define BMA180_IDENTITY 0x03 13 | #define BMA180_RESET_REGISTER 0x10 14 | #define BMA180_TRIGER_RESET_VALUE 0xB6 15 | #define BMA180_ENABLE_WRITE_CONTROL_REGISTER 0x0D 16 | #define BMA180_CONTROL_REGISTER 0x10 17 | #define BMA180_BW_TCS 0x20 18 | #define BMA180_LOW_PASS_FILTER_REGISTER 0x20 19 | #define BMA180_10HZ_LOW_PASS_FILTER_VALUE 0x0F 20 | #define BMA180_1200HZ_LOW_PASS_FILTER_VALUE 0x7F 21 | #define BMA180_OFFSET_REGISTER 0x35 22 | #define BMA180_READ_ROLL_ADDRESS 0x02 23 | #define BMA180_READ_PITCH_ADDRESS 0x04 24 | #define BMA180_READ_YAW_ADDRESS 0x06 25 | #define BMA180_BUFFER_SIZE 6 26 | 27 | float accel[3]; 28 | 29 | class BMA180 { 30 | public: 31 | // Constructor 32 | BMA180() { 33 | accelScaleFactor = 9.81 / 2048.0; 34 | 35 | accel_bias[XAXIS] = 0; 36 | accel_bias[YAXIS] = 0; 37 | accel_bias[ZAXIS] = 0; 38 | 39 | accelSamples = 0; 40 | }; 41 | 42 | void initialize() { 43 | // Check if sensor is alive 44 | Wire.beginTransmission(BMA180_ADDRESS); 45 | Wire.write(0x00); 46 | Wire.endTransmission(); 47 | 48 | Wire.requestFrom(BMA180_ADDRESS, 1); 49 | 50 | uint8_t register_value = Wire.read(); 51 | 52 | if (register_value == BMA180_IDENTITY) { 53 | sensors.sensors_detected |= ACCELEROMETER_DETECTED; 54 | } else { 55 | return; 56 | } 57 | 58 | initialize(); 59 | }; 60 | 61 | void initialize() { 62 | sensors.i2c_write8(BMA180_ADDRESS, BMA180_RESET_REGISTER, BMA180_TRIGER_RESET_VALUE); // reset 63 | delay(10); 64 | 65 | sensors.i2c_write8(BMA180_ADDRESS, BMA180_ENABLE_WRITE_CONTROL_REGISTER, BMA180_CONTROL_REGISTER); // enable writing to control registers 66 | 67 | Wire.beginTransmission(BMA180_ADDRESS); 68 | Wire.write(BMA180_BW_TCS); 69 | Wire.endTransmission(); 70 | 71 | Wire.requestFrom(BMA180_ADDRESS, 1); 72 | uint8_t data = Wire.read(); 73 | 74 | sensors.i2c_write8(BMA180_ADDRESS, BMA180_LOW_PASS_FILTER_REGISTER, data & BMA180_1200HZ_LOW_PASS_FILTER_VALUE); // set low pass filter to 1.2kHz (value = 0000xxxx) 75 | 76 | Wire.beginTransmission(BMA180_ADDRESS); 77 | Wire.write(BMA180_OFFSET_REGISTER); 78 | Wire.endTransmission(); 79 | 80 | Wire.requestFrom(BMA180_ADDRESS, 1); 81 | data = Wire.read(); 82 | data &= 0xF1; 83 | data |= 0x08; // set range select bits for +/-4g 84 | sensors.i2c_write8(BMA180_ADDRESS, BMA180_OFFSET_REGISTER, data); 85 | 86 | // setup axis mapping 87 | if (CONFIG.data.ACCEL_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 88 | CONFIG.data.ACCEL_AXIS_MAP.axis1 = 0; // x 89 | CONFIG.data.ACCEL_AXIS_MAP.axis2 = 1; // y 90 | CONFIG.data.ACCEL_AXIS_MAP.axis3 = 2; // z 91 | 92 | CONFIG.data.ACCEL_AXIS_MAP.axis1_sign = 1; 93 | CONFIG.data.ACCEL_AXIS_MAP.axis2_sign = 0; 94 | CONFIG.data.ACCEL_AXIS_MAP.axis3_sign = 0; 95 | 96 | CONFIG.data.ACCEL_AXIS_MAP.initialized = 1; 97 | 98 | // save default in eeprom 99 | writeEEPROM(); 100 | } 101 | }; 102 | 103 | // ~1280ms (only runs when requested) 104 | void calibrate_accel() { 105 | uint8_t i, count = 128; 106 | int32_t xSum = 0, ySum = 0, zSum = 0; 107 | 108 | for(i = 0; i < count; i++) { 109 | readAccelRaw(); 110 | xSum += accelRaw[XAXIS]; 111 | ySum += accelRaw[YAXIS]; 112 | zSum += accelRaw[ZAXIS]; 113 | delay(10); 114 | } 115 | 116 | CONFIG.data.ACCEL_BIAS[XAXIS] = xSum / count; 117 | CONFIG.data.ACCEL_BIAS[YAXIS] = ySum / count; 118 | CONFIG.data.ACCEL_BIAS[ZAXIS] = (zSum / count) - 2048; // - 1G 119 | 120 | // Reverse calibration forces 121 | CONFIG.data.ACCEL_BIAS[XAXIS] *= -1; 122 | CONFIG.data.ACCEL_BIAS[YAXIS] *= -1; 123 | CONFIG.data.ACCEL_BIAS[ZAXIS] *= -1; 124 | }; 125 | 126 | void readAccelRaw() { 127 | Wire.beginTransmission(BMA180_ADDRESS); 128 | Wire.write(BMA180_READ_ROLL_ADDRESS); 129 | Wire.endTransmission(); 130 | 131 | Wire.requestFrom(BMA180_ADDRESS, BMA180_BUFFER_SIZE); 132 | 133 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis1] = ((Wire.read() | (Wire.read() << 8)) >> 2) * (CONFIG.data.ACCEL_AXIS_MAP.axis1_sign?-1:1); 134 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis2] = ((Wire.read() | (Wire.read() << 8)) >> 2) * (CONFIG.data.ACCEL_AXIS_MAP.axis2_sign?-1:1); 135 | accelRaw[CONFIG.data.ACCEL_AXIS_MAP.axis3] = ((Wire.read() | (Wire.read() << 8)) >> 2) * (CONFIG.data.ACCEL_AXIS_MAP.axis3_sign?-1:1); 136 | }; 137 | 138 | void readAccelSum() { 139 | readAccelRaw(); 140 | 141 | accelSum[XAXIS] += accelRaw[XAXIS]; 142 | accelSum[YAXIS] += accelRaw[YAXIS]; 143 | accelSum[ZAXIS] += accelRaw[ZAXIS]; 144 | 145 | accelSamples++; 146 | }; 147 | 148 | void evaluateAccel() { 149 | // Calculate average 150 | accel[XAXIS] = accelSum[XAXIS] / accelSamples; 151 | accel[YAXIS] = accelSum[YAXIS] / accelSamples; 152 | accel[ZAXIS] = accelSum[ZAXIS] / accelSamples; 153 | 154 | // Apply offsets 155 | accel[XAXIS] += CONFIG.data.ACCEL_BIAS[XAXIS]; 156 | accel[YAXIS] += CONFIG.data.ACCEL_BIAS[YAXIS]; 157 | accel[ZAXIS] += CONFIG.data.ACCEL_BIAS[ZAXIS]; 158 | 159 | // Apply correct scaling (at this point accelNsumAvr reprensents +- 1g = 9.81 m/s^2) 160 | accel[XAXIS] *= accelScaleFactor; 161 | accel[YAXIS] *= accelScaleFactor; 162 | accel[ZAXIS] *= accelScaleFactor; 163 | 164 | // Reset SUM variables 165 | accelSum[XAXIS] = 0; 166 | accelSum[YAXIS] = 0; 167 | accelSum[ZAXIS] = 0; 168 | accelSamples = 0; 169 | }; 170 | 171 | private: 172 | float accelScaleFactor; 173 | 174 | int16_t accelRaw[3]; 175 | float accelSum[3]; 176 | 177 | uint8_t accelSamples; 178 | }; 179 | 180 | BMA180 bma; 181 | 182 | void SensorArray::initializeAccel() { 183 | bma.initialize(); 184 | } 185 | 186 | void SensorArray::calibrateAccel() { 187 | bma.calibrate_accel(); 188 | } 189 | 190 | void SensorArray::readAccelSum() { 191 | bma.readAccelSum(); 192 | } 193 | 194 | void SensorArray::evaluateAccel() { 195 | bma.evaluateAccel(); 196 | } 197 | -------------------------------------------------------------------------------- /libraries/P_Barometer/Barometer_bmp085.h: -------------------------------------------------------------------------------- 1 | /* BMP085 barometer implementation in C++ 2 | 3 | Big shoutout to whoever wrote the aeroquad BMP085 sensor integration 4 | (because it was used as example/base to create this one) 5 | */ 6 | 7 | // BMP085 Registers 8 | #define BMP085_ADDRESS 0x77 9 | 10 | // Operating Modes 11 | #define BMP085_ULTRALOWPOWER 0 12 | #define BMP085_STANDARD 1 13 | #define BMP085_HIGHRES 2 14 | #define BMP085_ULTRAHIGHRES 3 15 | 16 | #define BMP085_CAL_AC1 0xAA // R Calibration data (16 bits) 17 | #define BMP085_CAL_AC2 0xAC // R Calibration data (16 bits) 18 | #define BMP085_CAL_AC3 0xAE // R Calibration data (16 bits) 19 | #define BMP085_CAL_AC4 0xB0 // R Calibration data (16 bits) 20 | #define BMP085_CAL_AC5 0xB2 // R Calibration data (16 bits) 21 | #define BMP085_CAL_AC6 0xB4 // R Calibration data (16 bits) 22 | #define BMP085_CAL_B1 0xB6 // R Calibration data (16 bits) 23 | #define BMP085_CAL_B2 0xB8 // R Calibration data (16 bits) 24 | #define BMP085_CAL_MB 0xBA // R Calibration data (16 bits) 25 | #define BMP085_CAL_MC 0xBC // R Calibration data (16 bits) 26 | #define BMP085_CAL_MD 0xBE // R Calibration data (16 bits) 27 | 28 | #define BMP085_CONTROL 0xF4 29 | #define BMP085_TEMPDATA 0xF6 30 | #define BMP085_PRESSUREDATA 0xF6 31 | 32 | #define BMP085_READTEMPCMD 0x2E 33 | #define BMP085_READPRESSURECMD 0x34 34 | 35 | float baroRawAltitude = 0.0; 36 | float baroGroundAltitude = 0.0; 37 | float baroAltitude = 0.0; 38 | float baroAltitudeRunning = 0.0; 39 | 40 | float baroAltitudeToHoldTarget = 0.0; 41 | uint16_t baroAltitudeHoldThrottle = 1000; 42 | 43 | class BMP085 { 44 | public: 45 | // constructor 46 | BMP085() { 47 | }; 48 | 49 | void initialize() { 50 | baroAltitude = 0.0; 51 | 52 | overSamplingSetting = BMP085_STANDARD; 53 | pressureFactor = 1/5.255; 54 | baroSmoothFactor = 0.02; 55 | 56 | // Check if sensor is alive 57 | Wire.beginTransmission(BMP085_ADDRESS); 58 | Wire.write(0xD0); // BMP085_CHIP_ID_REG 59 | Wire.endTransmission(); 60 | 61 | Wire.requestFrom(BMP085_ADDRESS, 1); 62 | 63 | uint8_t register_value = Wire.read(); 64 | 65 | if (register_value == 0x55) { 66 | sensors.sensors_detected |= BAROMETER_DETECTED; 67 | } else { 68 | return; 69 | } 70 | 71 | // Read Calibration Data 72 | ac1 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC1); 73 | ac2 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC2); 74 | ac3 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC3); 75 | ac4 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC4); 76 | ac5 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC5); 77 | ac6 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_AC6); 78 | 79 | b1 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_B1); 80 | b2 = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_B2); 81 | 82 | mb = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_MB); 83 | mc = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_MC); 84 | md = sensors.i2c_read16(BMP085_ADDRESS, BMP085_CAL_MD); 85 | 86 | requestRawTemperature(); // setup up next measure() for temperature 87 | isReadPressure = false; 88 | 89 | pressureCount = 0; 90 | measureBaro(); 91 | 92 | delay(5); // delay for temperature 93 | measureBaro(); 94 | 95 | delay(10); // delay for pressure 96 | measureGroundBaro(); 97 | 98 | // check if measured ground altitude is valid 99 | while (abs(baroRawAltitude - baroGroundAltitude) > 10) { 100 | delay(26); 101 | measureGroundBaro(); 102 | } 103 | 104 | baroAltitude = baroGroundAltitude; 105 | }; 106 | 107 | void requestRawPressure() { 108 | sensors.i2c_write8(BMP085_ADDRESS, BMP085_CONTROL, BMP085_READPRESSURECMD + (overSamplingSetting << 6)); 109 | }; 110 | 111 | long readRawPressure() { 112 | Wire.beginTransmission(BMP085_ADDRESS); 113 | Wire.write(BMP085_PRESSUREDATA); 114 | Wire.endTransmission(); 115 | 116 | Wire.requestFrom(BMP085_ADDRESS, 3); 117 | 118 | return (((uint32_t)Wire.read() << 16) | ((uint32_t)Wire.read() << 8) | ((uint32_t)Wire.read())) >> (8 - overSamplingSetting); 119 | }; 120 | 121 | void requestRawTemperature() { 122 | sensors.i2c_write8(BMP085_ADDRESS, BMP085_CONTROL, BMP085_READTEMPCMD); 123 | }; 124 | 125 | uint16_t readRawTemperature() { 126 | uint16_t data; 127 | 128 | Wire.beginTransmission(BMP085_ADDRESS); 129 | Wire.write(BMP085_TEMPDATA); 130 | Wire.endTransmission(); 131 | 132 | Wire.requestFrom(BMP085_ADDRESS, 2); 133 | 134 | data = (Wire.read() << 8) | Wire.read(); 135 | 136 | return data; 137 | }; 138 | 139 | void measureBaro() { 140 | measureBaroSum(); 141 | evaluateBaroAltitude(); 142 | }; 143 | 144 | void measureBaroSum() { 145 | // switch between pressure and temperature measurements 146 | // each loop, since it is slow to measure pressure 147 | if (isReadPressure) { 148 | rawPressureSum += readRawPressure(); 149 | rawPressureSumCount++; 150 | 151 | if (pressureCount == 4) { 152 | requestRawTemperature(); 153 | pressureCount = 0; 154 | isReadPressure = false; 155 | } 156 | else { 157 | requestRawPressure(); 158 | } 159 | pressureCount++; 160 | } else { // select must equal TEMPERATURE 161 | rawTemperature = (long)readRawTemperature(); 162 | requestRawPressure(); 163 | isReadPressure = true; 164 | } 165 | }; 166 | 167 | void measureGroundBaro() { 168 | // measure initial ground pressure (multiple samples) 169 | float altSum = 0.0; 170 | for (uint8_t i = 0; i < 25; i++) { 171 | measureBaro(); 172 | altSum += baroRawAltitude; 173 | delay(12); 174 | } 175 | 176 | baroGroundAltitude = altSum / 25; 177 | }; 178 | 179 | void evaluateBaroAltitude() { 180 | int32_t x1, x2, x3, b3, b5, b6, p; 181 | uint32_t b4, b7; 182 | int32_t tmp; 183 | 184 | //calculate true temperature 185 | x1 = ((int32_t)rawTemperature - ac6) * ac5 >> 15; 186 | x2 = ((int32_t) mc << 11) / (x1 + md); 187 | b5 = x1 + x2; 188 | 189 | if (rawPressureSumCount == 0) { // it may occur at init time that no pressure has been read yet! 190 | return; 191 | } 192 | 193 | rawPressure = rawPressureSum / rawPressureSumCount; 194 | rawPressureSum = 0.0; 195 | rawPressureSumCount = 0; 196 | 197 | //calculate true pressure 198 | b6 = b5 - 4000; 199 | x1 = (b2 * (b6 * b6 >> 12)) >> 11; 200 | x2 = ac2 * b6 >> 11; 201 | x3 = x1 + x2; 202 | 203 | // Real Bosch formula - b3 = ((((int32_t)ac1 * 4 + x3) << overSamplingSetting) + 2) >> 2; 204 | // The version below is the same, but takes less program space 205 | tmp = ac1; 206 | tmp = (tmp * 4 + x3) << overSamplingSetting; 207 | b3 = (tmp + 2) >> 2; 208 | 209 | x1 = ac3 * b6 >> 13; 210 | x2 = (b1 * (b6 * b6 >> 12)) >> 16; 211 | x3 = ((x1 + x2) + 2) >> 2; 212 | b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; 213 | b7 = ((uint32_t) rawPressure - b3) * (50000 >> overSamplingSetting); 214 | p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; 215 | 216 | x1 = (p >> 8) * (p >> 8); 217 | x1 = (x1 * 3038) >> 16; 218 | x2 = (-7357 * p) >> 16; 219 | pressure = (p + ((x1 + x2 + 3791) >> 4)); 220 | 221 | baroRawAltitude = (1.0 - pow(pressure / 101325.0, pressureFactor)) * 44330.0; // returns absolute baroAltitude in meters 222 | baroAltitude = filterSmooth(baroRawAltitude, baroAltitude, baroSmoothFactor); 223 | }; 224 | 225 | void getBaroAltitude() { 226 | baroAltitudeRunning = baroAltitude - baroGroundAltitude; 227 | }; 228 | private: 229 | int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; 230 | uint16_t ac4, ac5, ac6; 231 | 232 | uint8_t overSamplingSetting; 233 | bool isReadPressure; 234 | 235 | float rawPressureSum; 236 | uint8_t rawPressureSumCount; 237 | 238 | int32_t pressure, rawPressure, rawTemperature; 239 | uint8_t pressureCount; 240 | 241 | float pressureFactor; 242 | float baroSmoothFactor; 243 | }; 244 | 245 | // Create Baro object 246 | BMP085 baro; 247 | 248 | void SensorArray::initializeBaro() { 249 | baro.initialize(); 250 | } 251 | 252 | void SensorArray::readBaroSum() { 253 | baro.measureBaroSum(); 254 | } 255 | 256 | void SensorArray::evaluateBaroAltitude() { 257 | baro.evaluateBaroAltitude(); 258 | baro.getBaroAltitude(); 259 | } 260 | -------------------------------------------------------------------------------- /libraries/P_Barometer/Barometer_ms5611.h: -------------------------------------------------------------------------------- 1 | /* MS5611 Barometer implementation in C++ 2 | 3 | Big shoutout to whoever wrote the aeroquad ms5611 sensor integration 4 | (because it was used as example/base to create this one) 5 | */ 6 | 7 | #define MS5611_I2C_ADDRESS 0x77 // address can also be 0x76 8 | 9 | #define MS561101BA_PROM_BASE_ADDR 0xA0 10 | #define MS561101BA_PROM_REG_COUNT 8 // number of registers in the PROM 11 | #define MS561101BA_D1_Pressure 0x40 12 | #define MS561101BA_D2_Temperature 0x50 13 | #define MS561101BA_RESET 0x1E 14 | 15 | #define MS561101BA_D1D2_SIZE 3 16 | 17 | // OSR (Over Sampling Ratio) constants 18 | #define MS561101BA_OSR_256 0x00 19 | #define MS561101BA_OSR_512 0x02 20 | #define MS561101BA_OSR_1024 0x04 21 | #define MS561101BA_OSR_2048 0x06 22 | #define MS561101BA_OSR_4096 0x08 23 | 24 | float baroRawAltitude = 0.0; 25 | float baroGroundAltitude = 0.0; 26 | float baroAltitude = 0.0; 27 | float baroAltitudeRunning = 0.0; 28 | 29 | float baroAltitudeToHoldTarget = 0.0; 30 | uint16_t baroAltitudeHoldThrottle = 1000; 31 | 32 | class MS5611 { 33 | public: 34 | // constructor 35 | MS5611() { 36 | }; 37 | 38 | void initialize() { 39 | baroAltitude = 0.0; 40 | 41 | pressureFactor = 1.0 / 5.255; 42 | baroSmoothFactor = 0.03; 43 | 44 | pressure = 0.0; 45 | 46 | // Reset the sensor (to populate its internal PROM registers) 47 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 48 | Wire.write(MS561101BA_RESET); 49 | Wire.endTransmission(); 50 | delay(100); 51 | 52 | // Check if sensor is alive & Read calibration data 53 | if (readPROM()) { 54 | sensors.sensors_detected |= BAROMETER_DETECTED; 55 | } else { 56 | return; 57 | } 58 | 59 | requestRawTemperature(); // setup up next measure() for temperature 60 | isReadPressure = false; 61 | pressureCount = 0; 62 | delay(10); 63 | measureBaroSum(); // read temperature 64 | delay(10); 65 | measureBaro(); // read pressure 66 | delay(10); 67 | 68 | measureGroundBaro(); 69 | delay(500); // Wait for sensor to heat up properly 70 | 71 | // check if measured ground altitude is valid 72 | while (abs(baroRawAltitude - baroGroundAltitude) > 4) { 73 | delay(26); 74 | measureGroundBaro(); 75 | } 76 | 77 | baroAltitude = baroGroundAltitude; 78 | }; 79 | 80 | void requestRawPressure() { 81 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 82 | Wire.write(MS561101BA_D1_Pressure + MS561101BA_OSR_4096); 83 | Wire.endTransmission(); 84 | } 85 | 86 | float readRawPressure() { 87 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 88 | Wire.write(0); 89 | Wire.endTransmission(); 90 | 91 | Wire.requestFrom(MS5611_I2C_ADDRESS, 3); 92 | 93 | lastRawPressure = (Wire.read() << 16) | (Wire.read() << 8) | (Wire.read() << 0); 94 | 95 | return (((lastRawPressure * sens) >> 21) - offset) >> 15; 96 | }; 97 | 98 | void requestRawTemperature() { 99 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 100 | Wire.write(MS561101BA_D2_Temperature + MS561101BA_OSR_4096); 101 | Wire.endTransmission(); 102 | }; 103 | 104 | uint32_t readRawTemperature() { 105 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 106 | Wire.write(0); 107 | Wire.endTransmission(); 108 | 109 | Wire.requestFrom(MS5611_I2C_ADDRESS, 3); 110 | 111 | lastRawTemperature = (Wire.read() << 16) | (Wire.read() << 8) | (Wire.read() << 0); 112 | 113 | int64_t dT = lastRawTemperature - (((int32_t)MS5611Prom[5]) << 8); 114 | offset = (((int64_t)MS5611Prom[2]) << 16) + ((MS5611Prom[4] * dT) >> 7); 115 | sens = (((int64_t)MS5611Prom[1]) << 15) + ((MS5611Prom[3] * dT) >> 8); 116 | 117 | return lastRawTemperature; 118 | }; 119 | 120 | void measureBaro() { 121 | measureBaroSum(); 122 | evaluateBaroAltitude(); 123 | }; 124 | 125 | void measureBaroSum() { 126 | // switch between pressure and temperature measurements 127 | // each loop, since it is slow to measure pressure 128 | if (isReadPressure) { 129 | rawPressureSum += readRawPressure(); 130 | rawPressureSumCount++; 131 | 132 | if (pressureCount == 20) { 133 | requestRawTemperature(); 134 | pressureCount = 0; 135 | isReadPressure = false; 136 | } 137 | else { 138 | requestRawPressure(); 139 | } 140 | pressureCount++; 141 | } else { // select must equal TEMPERATURE 142 | rawTemperature = (int32_t)readRawTemperature(); 143 | requestRawPressure(); 144 | isReadPressure = true; 145 | } 146 | }; 147 | 148 | void measureGroundBaro() { 149 | // measure initial ground pressure (multiple samples) 150 | float altSum = 0.0; 151 | for (uint8_t i = 0; i < 25; i++) { 152 | measureBaro(); 153 | altSum += baroRawAltitude; 154 | delay(12); 155 | } 156 | 157 | baroGroundAltitude = altSum / 25; 158 | }; 159 | 160 | void evaluateBaroAltitude() { 161 | if (rawPressureSumCount == 0) { // it may occur at init time that no pressure has been read yet! 162 | return; 163 | } 164 | 165 | pressure = rawPressureSum / rawPressureSumCount; 166 | rawPressureSum = 0.0; 167 | rawPressureSumCount = 0; 168 | 169 | baroRawAltitude = (1.0 - pow(pressure / 101325.0, pressureFactor)) * 44330.0; // returns absolute baroAltitude in centimeters 170 | 171 | baroAltitude = filterSmooth(baroRawAltitude, baroAltitude, baroSmoothFactor); 172 | }; 173 | 174 | void getBaroAltitude() { 175 | baroAltitudeRunning = baroAltitude - baroGroundAltitude; 176 | }; 177 | 178 | private: 179 | // Private variables 180 | uint16_t MS5611Prom[MS561101BA_PROM_REG_COUNT]; 181 | int64_t offset, sens; 182 | 183 | bool isReadPressure; 184 | float pressureFactor; 185 | float baroSmoothFactor; 186 | 187 | float rawPressureSum; 188 | uint8_t rawPressureSumCount; 189 | 190 | int32_t pressure, rawPressure, rawTemperature; 191 | uint8_t pressureCount; 192 | 193 | int32_t lastRawTemperature; 194 | int32_t lastRawPressure; 195 | 196 | // Taken from aeroquad 197 | uint8_t crc4(uint16_t n_prom[]) { 198 | uint16_t n_rem = 0; // crc reminder 199 | uint16_t crc_read; // original value of the crc 200 | 201 | crc_read = n_prom[7]; // save read CRC 202 | n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0 203 | 204 | for (int16_t cnt = 0; cnt < 16; cnt++) { // operation is performed on bytes 205 | // choose LSB or MSB 206 | if (cnt % 2 == 1) { 207 | n_rem ^= (n_prom[cnt >> 1]) & 0x00FF; 208 | } else { 209 | n_rem ^= n_prom[cnt >> 1] >> 8; 210 | } 211 | 212 | for (int16_t n_bit = 8; n_bit > 0; n_bit--) { 213 | if (n_rem & (0x8000)) { 214 | n_rem = (n_rem << 1) ^ 0x3000; 215 | } else { 216 | n_rem = (n_rem << 1); 217 | } 218 | } 219 | } 220 | 221 | n_rem = (n_rem >> 12) & 0xF; // // final 4-bit reminder is CRC code 222 | n_prom[7] = crc_read; // restore the crc_read to its original place 223 | 224 | return (n_rem); 225 | } 226 | 227 | // Taken from aeroquad 228 | int16_t readPROM() { 229 | for (int16_t i = 0; i < MS561101BA_PROM_REG_COUNT; i++) { 230 | Wire.beginTransmission(MS5611_I2C_ADDRESS); 231 | Wire.write(MS561101BA_PROM_BASE_ADDR + 2 * i); 232 | Wire.endTransmission(); 233 | 234 | if (Wire.requestFrom(MS5611_I2C_ADDRESS, 2) == 2) { 235 | MS5611Prom[i] = (Wire.read() << 8) | Wire.read(); 236 | } else { 237 | return 0; 238 | } 239 | } 240 | 241 | int16_t crc = crc4(MS5611Prom); 242 | int16_t crcProm = MS5611Prom[7] & 0xf; 243 | if (crc == crcProm) { 244 | return 1; 245 | } 246 | return 0; 247 | } 248 | }; 249 | 250 | // Create Baro object 251 | MS5611 baro; 252 | 253 | void SensorArray::initializeBaro() { 254 | baro.initialize(); 255 | } 256 | 257 | void SensorArray::readBaroSum() { 258 | baro.measureBaroSum(); 259 | } 260 | 261 | void SensorArray::evaluateBaroAltitude() { 262 | baro.evaluateBaroAltitude(); 263 | baro.getBaroAltitude(); 264 | } 265 | -------------------------------------------------------------------------------- /libraries/P_BatteryMonitor/BatteryMonitor_current.h: -------------------------------------------------------------------------------- 1 | /* Current monitor based on ACS758X 100B Hall Effect Linear Current Sensor 2 | 3 | Could use some optimizations and better documentation. 4 | */ 5 | #define CURRENT_SENSOR_PIN 17 6 | #define CURRENT_SENSOR_ZERO 512 // no current on the load (default 512) 7 | #define CURRENT_SENSOR_FULL_SCALE_V 3.3 8 | #define CURRENT_SENSOR_SENSITIVITY_5V 0.02 9 | #define CURRENT_SENSOR_SENSITIVITY_3V3 0.0132 10 | 11 | int16_t current; 12 | float currentAmps; 13 | float batteryMonitorCapacityUsed = 0.0; 14 | unsigned long batteryMonitorCurrent_timer; 15 | 16 | void readBatteryMonitorCurrent() { 17 | // Current time 18 | unsigned long now = millis(); 19 | float dT = 1000.0 / (now - batteryMonitorCurrent_timer); // Delta Time = 1 second / time passed = Hz 20 | 21 | current = analogRead(CURRENT_SENSOR_PIN); 22 | 23 | // Dead band 24 | if (abs(current) - CURRENT_SENSOR_ZERO > 1) { 25 | currentAmps = (current - CURRENT_SENSOR_ZERO) * CURRENT_SENSOR_FULL_SCALE_V / 1024.0 / CURRENT_SENSOR_SENSITIVITY_3V3; 26 | 27 | // Update capacity used with new data 28 | batteryMonitorCapacityUsed += (currentAmps / dT); 29 | 30 | // Save time for next comparison 31 | batteryMonitorCurrent_timer = now; 32 | } else { 33 | currentAmps = 0.0; 34 | } 35 | } 36 | 37 | float getBatteryMonitorCapacityUsed() { 38 | // batteryMonitorCapacityUsed is saved in A / hour, in here we will multiply the saved value by 1000 to convert from A to mA 39 | // divide that by 60 = mAps / minute 40 | // and another division by 60 will get the output to mAps / hour. 41 | return batteryMonitorCapacityUsed * 1000.0 / 60.0 / 60.0; 42 | } 43 | 44 | float getBatteryMonitorLiveCurrent() { 45 | return currentAmps; 46 | } 47 | -------------------------------------------------------------------------------- /libraries/P_BatteryMonitor/BatteryMonitor_voltage.h: -------------------------------------------------------------------------------- 1 | /* 2 | Simple Battery voltage monitor 3 | by cTn 4 | 5 | AnalogRead scale on Teensy 3.0 is 3.3 / 1024 = 0.0032 6 | My "default" voltage divider is using 10K R1 and 1K5 R2 with 100nF capacitor paralel to R2 7 | Voltage divider scale is (10 + 1.5) / 1.5 = 13% 8 | Real voltage = ADC scale / (13% * 100) 9 | 10 | Measured values will be always averaged over 5 samples 11 | */ 12 | 13 | // Battery voltage monitor PIN 14 | #define BAT_V_MONITOR_PIN 15 15 | #define BAT_V_MONITOR_WARNING 10.5 16 | #define BAT_V_MONITOR_ALARM 9.9 17 | 18 | #define BAT_V_MONITOR_SCALE 0.0032 19 | #define BAT_V_MONITOR_SCALE_FACTOR (BAT_V_MONITOR_SCALE / 0.1304) 20 | 21 | float BatteryVoltage = 0; 22 | bool BatteryWarning = false; 23 | bool BatteryAlarm = false; 24 | 25 | float BatteryVoltageSum = 0; 26 | uint8_t BatteryVoltageSamples = 0; 27 | 28 | // Variables used for warning/alarm indication via orientation lights 29 | uint8_t BatteryBlinkCounter = 0; 30 | bool BatteryBlinkState = false; 31 | 32 | void measureBatteryVoltage() { 33 | // Read analog PIN value into variable 34 | BatteryVoltageSum += analogRead(BAT_V_MONITOR_PIN); 35 | 36 | BatteryVoltageSamples++; 37 | 38 | if (BatteryVoltageSamples >= 5) { 39 | // Calculate Average 40 | BatteryVoltageSum = BatteryVoltageSum / BatteryVoltageSamples; 41 | 42 | // Properly scale it 43 | BatteryVoltage = BatteryVoltageSum * BAT_V_MONITOR_SCALE_FACTOR; 44 | 45 | // Reset SUM variables 46 | BatteryVoltageSum = 0; 47 | BatteryVoltageSamples = 0; 48 | 49 | // Warning & critical battery voltage flag handling 50 | if (BatteryVoltage < BAT_V_MONITOR_ALARM) { 51 | // Battery critical 52 | BatteryAlarm = true; 53 | } else if (BatteryVoltage < BAT_V_MONITOR_WARNING) { 54 | // Battery low 55 | BatteryWarning = true; 56 | } else { 57 | // Reset flags to "OFF" state 58 | BatteryAlarm = false; 59 | BatteryWarning = false; 60 | } 61 | 62 | #ifdef DISABLE_BATTERY_ALARM 63 | BatteryAlarm = false; 64 | BatteryWarning = false; 65 | #endif 66 | } 67 | } 68 | 69 | /* This code should be in the main setup() 70 | pinMode(BAT_V_MONITOR_PIN, INPUT); // Battery voltage input pin 71 | */ 72 | 73 | /* This code should be entered inside the 50Hz loop 74 | 75 | // Orientation lights are also used to indicate battery voltage during flight 76 | // Warning = slow blinking 77 | // Alarm = fast blinking 78 | if (BatteryAlarm || BatteryWarning) { 79 | BatteryBlinkCounter++; 80 | 81 | uint8_t BlinkSpeed = 6; // Default blink speed (for battery warning) 82 | if (BatteryAlarm) BlinkSpeed = 1; // Fast blink speed (for battery critical) 83 | 84 | if (BatteryBlinkCounter >= BlinkSpeed) { 85 | BatteryBlinkState = !BatteryBlinkState; 86 | BatteryBlinkCounter = 0; 87 | 88 | digitalWrite(LED_ORIENTATION, BatteryBlinkState); 89 | } 90 | } else { 91 | digitalWrite(LED_ORIENTATION, HIGH); // set to HIGH by default 92 | } 93 | 94 | */ -------------------------------------------------------------------------------- /libraries/P_ESC/ESC_328p_HW.h: -------------------------------------------------------------------------------- 1 | /* ESC signal generation via shared timer1. 2 | 3 | MotorOuts array was added to separate variables changed by the FC in real time and PWM motor variables 4 | that are used inside the interrupt (without this, unpleasant behavior could occrur while system is writing to the 5 | MotorOut array and timer triggers interrupt). 6 | 7 | Big thanks to kha from #aeroquad for setting up the shared timer. 8 | 9 | This section is unfinished, default motor amount is still hardcoded to 4 10 | */ 11 | 12 | uint16_t MotorOuts[MOTORS]; 13 | uint8_t motorCounter = 0; 14 | uint16_t motorTotal = 0; 15 | uint8_t motorPins[8] = {2, 3, 4, 5, 6, 7, 9, 10}; // pin 8 is used as ppm input (thats why its skipped in this sequence) 16 | 17 | // Comparison Interrupt Vector 18 | ISR(TIMER1_COMPA_vect) { 19 | if (motorCounter < MOTORS) { 20 | digitalWrite(motorPins[motorCounter], 0); // end previous channel 21 | } 22 | 23 | motorCounter = (motorCounter + 1) % (MOTORS + 1); // advance to next 24 | 25 | if (motorCounter < MOTORS) { 26 | unsigned int now = MotorOuts[motorCounter] * 2; 27 | 28 | if (now < 2000) now = 2000; // sanity 29 | else if (now > 4000) now = 4000; // sanity 30 | 31 | motorTotal += now; // tally up time 32 | OCR1A = TCNT1 + now; // interrupt 33 | digitalWrite(motorPins[motorCounter], 1); // start pulse 34 | } else { // motorCounter == MOTORS 35 | OCR1A = TCNT1 + MOTORS * 4000 + 50 - motorTotal; 36 | motorTotal = 0; 37 | } 38 | } 39 | 40 | // Timer1 setup (shared timer for PPM sampling & PWM servo signal generation) 41 | void setupTimer1Esc() { 42 | // Setup timer1 in normal mode, count at 2MHz 43 | TCCR1A = 0; 44 | TCCR1B = (1 << CS11) | (1 << ICES1); 45 | TIMSK1 |= (1 << ICIE1) | (1 << OCIE1A); // Enable ICP and OCRA interrupts 46 | } 47 | 48 | void updateMotors() { 49 | cli(); // disable interrupts 50 | 51 | #if MOTORS == 3 52 | MotorOuts[0] = MotorOut[0]; 53 | MotorOuts[1] = MotorOut[1]; 54 | MotorOuts[2] = MotorOut[2]; 55 | #elif MOTORS == 4 56 | MotorOuts[0] = MotorOut[0]; 57 | MotorOuts[1] = MotorOut[1]; 58 | MotorOuts[2] = MotorOut[2]; 59 | MotorOuts[3] = MotorOut[3]; 60 | #elif MOTORS == 6 61 | MotorOuts[0] = MotorOut[0]; 62 | MotorOuts[1] = MotorOut[1]; 63 | MotorOuts[2] = MotorOut[2]; 64 | MotorOuts[3] = MotorOut[3]; 65 | MotorOuts[4] = MotorOut[4]; 66 | MotorOuts[5] = MotorOut[5]; 67 | #elif MOTORS == 8 68 | MotorOuts[0] = MotorOut[0]; 69 | MotorOuts[1] = MotorOut[1]; 70 | MotorOuts[2] = MotorOut[2]; 71 | MotorOuts[3] = MotorOut[3]; 72 | MotorOuts[4] = MotorOut[4]; 73 | MotorOuts[5] = MotorOut[5]; 74 | MotorOuts[6] = MotorOut[6]; 75 | MotorOuts[7] = MotorOut[7]; 76 | #endif 77 | 78 | sei(); // enable interrupts 79 | } 80 | 81 | void initializeESC() { 82 | // We will also initialize the separate MotorOuts array values here 83 | // PWM pins are also set to output over here 84 | for (uint8_t i = 0; i < MOTORS; i++) { 85 | MotorOuts[i] = MotorOut[i]; 86 | pinMode(motorPins[i], OUTPUT); 87 | } 88 | 89 | // Standard timer initialization 90 | setupTimer1Esc(); 91 | 92 | if (CONFIG.data.calibrateESC) { 93 | // Calibration sequence requested 94 | 95 | // Signal range TOP maximum 96 | for (uint8_t motor = 0; motor < MOTORS; motor++) { 97 | MotorOut[motor] = 2000; 98 | } 99 | updateMotors(); 100 | 101 | // Wait for all ESCs to acknowledge (1 beep) 102 | delay(5000); 103 | 104 | // Signal range BOTTOM minimum 105 | for (uint8_t motor = 0; motor < MOTORS; motor++) { 106 | MotorOut[motor] = 1000; 107 | } 108 | updateMotors(); 109 | 110 | // Wait for all ESCs to acknowledge (2 + 1 beep) 111 | delay(4000); 112 | 113 | // Calibration done 114 | // disabling the calibration flag and updating EEPROM 115 | CONFIG.data.calibrateESC = 0; 116 | writeEEPROM(); 117 | } 118 | } -------------------------------------------------------------------------------- /libraries/P_ESC/ESC_teensy3_HW.h: -------------------------------------------------------------------------------- 1 | /* ESC / Servo signal generation done in hardware by FLEX timer, without ISR (yay!) 2 | 3 | We are using flex timer0 which supports 8 channels. 4 | 5 | Currently generating 400 Hz PWM signal that is fed into electronic speed controllers 6 | corresponding to each rotor. 7 | 8 | This code will probably be expanded to also generate servo signal for 9 | gimbal stabilization (hopefully in near future). 10 | 11 | Big thanks to kha from #aeroquad for helping me get this up and running. 12 | 13 | == PIN Configuration == 14 | 15 | Channel - PIN name - Teensy 3.0 PIN numbering 16 | 17 | FTM0_CH0 - PTC1 - 22 18 | FTM0_CH1 - PTC2 - 23 19 | FTM0_CH2 - PTC3 - 9 20 | FTM0_CH3 - PTC4 - 10 21 | FTM0_CH4 - PTD4 - 6 22 | FTM0_CH5 - PTD5 - 20 23 | FTM0_CH6 - PTD6 - 21 24 | FTM0_CH7 - PTD7 - 5 25 | */ 26 | 27 | void setupFTM0() { 28 | // Flex timer0 configuration 29 | FTM0_SC = 0x0c; // TOF=0 TOIE=0 CPWMS=0 CLKS=01 PS=100 (divide by 16) 30 | 31 | #ifdef ESC_400HZ 32 | // 400Hz PWM signal 33 | FTM0_MOD = 7500; 34 | #else 35 | // 250Hz PWM signal 36 | FTM0_MOD = 12000; 37 | #endif 38 | 39 | FTM0_C0SC = 0x28; 40 | 41 | // Initial values (3000 = 1ms) 42 | #if MOTORS == 3 43 | FTM0_C0V = 3000; 44 | FTM0_C1V = 3000; 45 | FTM0_C2V = 3000; 46 | 47 | // PORT Configuration 48 | PORTC_PCR1 |= 0x400; 49 | PORTC_PCR2 |= 0x400; 50 | PORTC_PCR3 |= 0x400; 51 | #elif MOTORS == 4 52 | FTM0_C0V = 3000; 53 | FTM0_C1V = 3000; 54 | FTM0_C2V = 3000; 55 | FTM0_C3V = 3000; 56 | 57 | // PORT Configuration 58 | PORTC_PCR1 |= 0x400; 59 | PORTC_PCR2 |= 0x400; 60 | PORTC_PCR3 |= 0x400; 61 | PORTC_PCR4 |= 0x400; 62 | #elif MOTORS == 6 63 | FTM0_C0V = 3000; 64 | FTM0_C1V = 3000; 65 | FTM0_C2V = 3000; 66 | FTM0_C3V = 3000; 67 | FTM0_C4V = 3000; 68 | FTM0_C5V = 3000; 69 | 70 | // PORT Configuration 71 | PORTC_PCR1 |= 0x400; 72 | PORTC_PCR2 |= 0x400; 73 | PORTC_PCR3 |= 0x400; 74 | PORTC_PCR4 |= 0x400; 75 | PORTD_PCR4 |= 0x400; 76 | PORTD_PCR5 |= 0x400; 77 | #elif MOTORS == 8 78 | FTM0_C0V = 3000; 79 | FTM0_C1V = 3000; 80 | FTM0_C2V = 3000; 81 | FTM0_C3V = 3000; 82 | FTM0_C4V = 3000; 83 | FTM0_C5V = 3000; 84 | FTM0_C6V = 3000; 85 | FTM0_C7V = 3000; 86 | 87 | // PORT Configuration 88 | PORTC_PCR1 |= 0x400; 89 | PORTC_PCR2 |= 0x400; 90 | PORTC_PCR3 |= 0x400; 91 | PORTC_PCR4 |= 0x400; 92 | PORTD_PCR4 |= 0x400; 93 | PORTD_PCR5 |= 0x400; 94 | PORTD_PCR6 |= 0x400; 95 | PORTD_PCR7 |= 0x400; 96 | #endif 97 | } 98 | 99 | void updateMotors() { 100 | #if MOTORS == 3 101 | FTM0_C0V = MotorOut[0] * 3; 102 | FTM0_C1V = MotorOut[1] * 3; 103 | FTM0_C2V = MotorOut[2] * 3; 104 | #elif MOTORS == 4 105 | FTM0_C0V = MotorOut[0] * 3; 106 | FTM0_C1V = MotorOut[1] * 3; 107 | FTM0_C2V = MotorOut[2] * 3; 108 | FTM0_C3V = MotorOut[3] * 3; 109 | #elif MOTORS == 6 110 | FTM0_C0V = MotorOut[0] * 3; 111 | FTM0_C1V = MotorOut[1] * 3; 112 | FTM0_C2V = MotorOut[2] * 3; 113 | FTM0_C3V = MotorOut[3] * 3; 114 | FTM0_C4V = MotorOut[4] * 3; 115 | FTM0_C5V = MotorOut[5] * 3; 116 | #elif MOTORS == 8 117 | FTM0_C0V = MotorOut[0] * 3; 118 | FTM0_C1V = MotorOut[1] * 3; 119 | FTM0_C2V = MotorOut[2] * 3; 120 | FTM0_C3V = MotorOut[3] * 3; 121 | FTM0_C4V = MotorOut[4] * 3; 122 | FTM0_C5V = MotorOut[5] * 3; 123 | FTM0_C6V = MotorOut[6] * 3; 124 | FTM0_C7V = MotorOut[7] * 3; 125 | #endif 126 | } 127 | 128 | void initializeESC() { 129 | setupFTM0(); 130 | 131 | if (CONFIG.data.calibrateESC) { 132 | // Calibration sequence requested 133 | 134 | // Signal range TOP maximum 135 | for (uint8_t motor = 0; motor < MOTORS; motor++) { 136 | MotorOut[motor] = 2000; 137 | } 138 | updateMotors(); 139 | 140 | // Wait for all ESCs to acknowledge (1 beep) 141 | delay(5000); 142 | 143 | // Signal range BOTTOM minimum 144 | for (uint8_t motor = 0; motor < MOTORS; motor++) { 145 | MotorOut[motor] = 1000; 146 | } 147 | updateMotors(); 148 | 149 | // Wait for all ESCs to acknowledge (2 + 1 beep) 150 | delay(4000); 151 | 152 | // Calibration done 153 | // disabling the calibration flag and updating EEPROM 154 | CONFIG.data.calibrateESC = 0; 155 | writeEEPROM(); 156 | } 157 | } -------------------------------------------------------------------------------- /libraries/P_FrameType/FrameType_QuadPlus.h: -------------------------------------------------------------------------------- 1 | /* 2 | FRONT 3 | CW (0) 4 | 5 | O 6 | | 7 | | 8 | | 9 | CCW (3) O-----#-----O CCW (1) 10 | LEFT | RIGHT 11 | | 12 | | 13 | O 14 | 15 | CW (2) 16 | BACK 17 | */ 18 | 19 | #define MOTORS 4 20 | uint16_t MotorOut[MOTORS] = {1000, 1000, 1000, 1000}; 21 | 22 | void updateMotorsMix() { 23 | // Limit YAW to +- 200 (20%) 24 | YawMotorSpeed = constrain(YawMotorSpeed, -200, 200); 25 | 26 | // All of the motor outputs are constrained to standard 1000 - 2000 us PWM 27 | // Minimum Armed Throttle variable was added to prevent motors stopping during flight (in extreme "stabilization" behaviours) 28 | MotorOut[0] = constrain(throttle + PitchMotorSpeed + YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 29 | MotorOut[1] = constrain(throttle - RollMotorSpeed - YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 30 | MotorOut[2] = constrain(throttle - PitchMotorSpeed + YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 31 | MotorOut[3] = constrain(throttle + RollMotorSpeed - YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 32 | } -------------------------------------------------------------------------------- /libraries/P_FrameType/FrameType_QuadX.h: -------------------------------------------------------------------------------- 1 | /* 2 | FRONT 3 | CW (0) CCW (1) 4 | 5 | O O 6 | \ / 7 | \ / 8 | \ / 9 | \ / 10 | LEFT # RIGHT 11 | / \ 12 | / \ 13 | / \ 14 | / \ 15 | O O 16 | 17 | CCW (3) CW (2) 18 | BACK 19 | */ 20 | 21 | #define MOTORS 4 22 | uint16_t MotorOut[MOTORS] = {1000, 1000, 1000, 1000}; 23 | 24 | void updateMotorsMix() { 25 | // Limit YAW to +- 200 (20%) 26 | YawMotorSpeed = constrain(YawMotorSpeed, -200, 200); 27 | 28 | // All of the motor outputs are constrained to standard 1000 - 2000 us PWM 29 | // Minimum Armed Throttle variable was added to prevent motors stopping during flight (in extreme "stabilization" behaviours) 30 | MotorOut[0] = constrain(throttle + PitchMotorSpeed + RollMotorSpeed + YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 31 | MotorOut[1] = constrain(throttle + PitchMotorSpeed - RollMotorSpeed - YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 32 | MotorOut[2] = constrain(throttle - PitchMotorSpeed - RollMotorSpeed + YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 33 | MotorOut[3] = constrain(throttle - PitchMotorSpeed + RollMotorSpeed - YawMotorSpeed, CONFIG.data.minimumArmedThrottle, 2000); 34 | } -------------------------------------------------------------------------------- /libraries/P_GPS/neo-6m_configuration.txt: -------------------------------------------------------------------------------- 1 | MON-VER - 0A 04 28 00 37 2E 30 33 20 28 34 35 39 36 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 2 | CFG-ANT - 06 13 04 00 1B 00 8B A9 3 | CFG-DAT - 06 06 02 00 00 00 4 | CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 5 | CFG-INF - 06 02 0A 00 00 00 00 00 FF FF FF FF 00 00 6 | CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 7 | CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 8 | CFG-ITFM - 06 39 08 00 F3 AC 62 2D 1E 03 00 00 9 | CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 10 | CFG-MSG - 06 01 08 00 01 02 00 01 00 00 00 00 11 | CFG-MSG - 06 01 08 00 01 03 00 01 00 00 00 00 12 | CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 13 | CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 14 | CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 15 | CFG-MSG - 06 01 08 00 01 12 00 01 00 00 00 00 16 | CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 17 | CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 18 | CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 19 | CFG-MSG - 06 01 08 00 01 30 00 01 00 00 00 00 20 | CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 21 | CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 22 | CFG-MSG - 06 01 08 00 02 20 00 1E 00 00 00 00 23 | CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 24 | CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 25 | CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 26 | CFG-MSG - 06 01 08 00 0A 06 00 01 00 00 00 00 27 | CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 28 | CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 29 | CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 30 | CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 31 | CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 32 | CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 33 | CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 34 | CFG-MSG - 06 01 08 00 0B 05 00 00 00 00 00 00 35 | CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 36 | CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 37 | CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 38 | CFG-MSG - 06 01 08 00 0B 33 00 00 00 00 00 00 39 | CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 40 | CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 41 | CFG-MSG - 06 01 08 00 0D 06 00 00 00 00 00 00 42 | CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 43 | CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 44 | CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 45 | CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 46 | CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 47 | CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 48 | CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 49 | CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 50 | CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 51 | CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 52 | CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 53 | CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 54 | CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 55 | CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 56 | CFG-NAV5 - 06 24 24 00 FF FF 00 03 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 57 | CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 58 | CFG-NMEA - 06 17 04 00 00 23 00 02 59 | CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 60 | CFG-PM2 - 06 3B 2C 00 01 06 00 00 00 90 02 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 2C 01 00 00 4F C1 03 00 86 02 00 00 FE 00 00 00 64 40 01 00 61 | CFG-PRT - 06 00 14 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 62 | CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 96 00 00 01 00 01 00 00 00 00 00 63 | CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 64 | CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 65 | CFG-PRT - 06 00 14 00 04 00 00 00 00 32 00 00 00 00 00 00 07 00 07 00 00 00 00 00 66 | CFG-RATE - 06 08 06 00 C8 00 01 00 01 00 67 | CFG-RINV - 06 34 18 00 00 4E 6F 74 69 63 65 3A 20 6E 6F 20 64 61 74 61 20 73 61 76 65 64 21 00 68 | CFG-RXM - 06 11 02 00 08 00 69 | CFG-SBAS - 06 16 08 00 01 03 03 00 51 62 06 00 70 | CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 71 | CFG-TP5 - 06 31 20 00 00 00 00 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 72 | CFG-TP5 - 06 31 20 00 01 00 00 00 32 00 00 00 04 00 00 00 01 00 00 00 48 E8 01 00 A0 86 01 00 00 00 00 00 FE 00 00 00 73 | CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 74 | -------------------------------------------------------------------------------- /libraries/P_Gyroscope/ITG3200_AQ_v20.h: -------------------------------------------------------------------------------- 1 | /* ITG3200 - on AQ shield v 2.0 2 | (ITG3200 is on separate breakout) 3 | 4 | This library requires some more documentation 5 | */ 6 | 7 | #define ITG3200_ADDRESS 0x69 8 | 9 | #define ITG3200_IDENTITY 0x68 10 | #define ITG3200_IDENTITY_MASK 0x7E 11 | #define ITG3200_MEMORY_ADDRESS 0x1D 12 | #define ITG3200_BUFFER_SIZE 6 13 | #define ITG3200_RESET_ADDRESS 0x3E 14 | #define ITG3200_RESET_VALUE 0x80 15 | #define ITG3200_LOW_PASS_FILTER_ADDR 0x16 16 | #define ITG3200_LOW_PASS_FILTER_VALUE 0x1D // 10Hz low pass filter 17 | #define ITG3200_OSCILLATOR_ADDR 0x3E 18 | #define ITG3200_OSCILLATOR_VALUE 0x01 // use X gyro oscillator 19 | #define ITG3200_SCALE_TO_RADIANS 823.626831 // 14.375 LSBs per °/sec, / Pi / 180 20 | #define ITG3200_TEMPERATURE_ADDRESS 0x1B 21 | 22 | float gyro[3]; 23 | 24 | class ITG3200 { 25 | public: 26 | // Constructor 27 | ITG3200() { 28 | gyroScaleFactor = radians(1.0 / 14.375); // ITG3200 14.375 LSBs per °/sec 29 | 30 | gyroSamples = 0; 31 | }; 32 | 33 | void initialize() { 34 | // Check if sensor is alive 35 | Wire.beginTransmission(ITG3200_ADDRESS); 36 | Wire.write(0x00); 37 | Wire.endTransmission(); 38 | 39 | Wire.requestFrom(ITG3200_ADDRESS, 1); 40 | 41 | uint8_t register_value = Wire.read(); 42 | 43 | if ((register_value & ITG3200_IDENTITY_MASK) == ITG3200_IDENTITY) { 44 | sensors.sensors_detected |= GYROSCOPE_DETECTED; 45 | } else { 46 | return; 47 | } 48 | 49 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_RESET_ADDRESS, ITG3200_RESET_VALUE); // send a reset to the device 50 | 51 | // Startup delay 52 | delay(100); 53 | 54 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_LOW_PASS_FILTER_ADDR, ITG3200_LOW_PASS_FILTER_VALUE); // 10Hz low pass filter 55 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_RESET_ADDRESS, ITG3200_OSCILLATOR_VALUE); // use internal oscillator 56 | 57 | // Let the gyro heat up 58 | delay(1500); 59 | 60 | // setup axis mapping 61 | if (CONFIG.data.GYRO_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 62 | CONFIG.data.GYRO_AXIS_MAP.axis1 = 0; // x 63 | CONFIG.data.GYRO_AXIS_MAP.axis2 = 1; // y 64 | CONFIG.data.GYRO_AXIS_MAP.axis3 = 2; // z 65 | 66 | CONFIG.data.GYRO_AXIS_MAP.axis1_sign = 0; 67 | CONFIG.data.GYRO_AXIS_MAP.axis2_sign = 1; 68 | CONFIG.data.GYRO_AXIS_MAP.axis3_sign = 0; 69 | 70 | CONFIG.data.GYRO_AXIS_MAP.initialized = 1; 71 | 72 | // save default in eeprom 73 | writeEEPROM(); 74 | } 75 | }; 76 | 77 | // ~1280ms 78 | void calibrate_gyro() { 79 | static uint8_t retry = 0; 80 | uint8_t i, count = 128; 81 | int16_t xSum = 0, ySum = 0, zSum = 0; 82 | 83 | for(i = 0; i < count; i++) { 84 | readGyroRaw(); 85 | xSum += gyroRaw[XAXIS]; 86 | ySum += gyroRaw[YAXIS]; 87 | zSum += gyroRaw[ZAXIS]; 88 | delay(10); 89 | } 90 | 91 | gyro_offset[XAXIS] = -xSum / count; 92 | gyro_offset[YAXIS] = -ySum / count; 93 | gyro_offset[ZAXIS] = -zSum / count; 94 | 95 | // Calibration sanity check 96 | // if suitable offset couldn't be established, break out of the loop after 10 retries 97 | if ((abs(gyro_offset[XAXIS]) > 140 || abs(gyro_offset[YAXIS]) > 140 || abs(gyro_offset[ZAXIS]) > 140) && retry < 10) { 98 | // gyro calibration failed, run again 99 | retry++; 100 | 101 | // small delay before next gyro calibration 102 | delay(500); 103 | 104 | calibrate_gyro(); 105 | } 106 | }; 107 | 108 | void readGyroRaw() { 109 | Wire.beginTransmission(ITG3200_ADDRESS); 110 | Wire.write(ITG3200_MEMORY_ADDRESS); 111 | Wire.endTransmission(); 112 | 113 | Wire.requestFrom(ITG3200_ADDRESS, ITG3200_BUFFER_SIZE); 114 | 115 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis1] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis1_sign?-1:1); 116 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis2] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis2_sign?-1:1); 117 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis3] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis3_sign?-1:1); 118 | }; 119 | 120 | void readGyroSum() { 121 | readGyroRaw(); 122 | 123 | gyroSum[XAXIS] += gyroRaw[XAXIS]; 124 | gyroSum[YAXIS] += gyroRaw[YAXIS]; 125 | gyroSum[ZAXIS] += gyroRaw[ZAXIS]; 126 | 127 | gyroSamples++; 128 | }; 129 | 130 | void evaluateGyro() { 131 | // Calculate average 132 | gyro[XAXIS] = gyroSum[XAXIS] / gyroSamples; 133 | gyro[YAXIS] = -(gyroSum[YAXIS] / gyroSamples); 134 | gyro[ZAXIS] = gyroSum[ZAXIS] / gyroSamples; 135 | 136 | // Apply offsets 137 | gyro[XAXIS] += gyro_offset[XAXIS]; 138 | gyro[YAXIS] += gyro_offset[YAXIS]; 139 | gyro[ZAXIS] += gyro_offset[ZAXIS]; 140 | 141 | // Apply correct scaling (at this point gyroNsumRate is in radians) 142 | gyro[XAXIS] *= gyroScaleFactor; 143 | gyro[YAXIS] *= gyroScaleFactor; 144 | gyro[ZAXIS] *= gyroScaleFactor; 145 | 146 | // Reset SUM variables 147 | gyroSum[XAXIS] = 0; 148 | gyroSum[YAXIS] = 0; 149 | gyroSum[ZAXIS] = 0; 150 | gyroSamples = 0; 151 | }; 152 | private: 153 | int16_t gyro_offset[3]; 154 | int16_t gyroRaw[3]; 155 | float gyroSum[3]; 156 | float gyroScaleFactor; 157 | 158 | uint8_t gyroSamples; 159 | }; 160 | 161 | ITG3200 itg; 162 | 163 | void SensorArray::initializeGyro() { 164 | itg.initialize(); 165 | itg.calibrate_gyro(); 166 | } 167 | 168 | void SensorArray::readGyroSum() { 169 | itg.readGyroSum(); 170 | } 171 | 172 | void SensorArray::evaluateGyro() { 173 | itg.evaluateGyro(); 174 | } -------------------------------------------------------------------------------- /libraries/P_Gyroscope/ITG3200_AQ_v21.h: -------------------------------------------------------------------------------- 1 | /* ITG3200, 9DOF stick library 2 | 3 | This library requires some more documentation 4 | (code was tested and works perfectly fine) 5 | */ 6 | 7 | #define ITG3200_ADDRESS 0x68 8 | 9 | #define ITG3200_IDENTITY 0x68 10 | #define ITG3200_IDENTITY_MASK 0x7E 11 | #define ITG3200_MEMORY_ADDRESS 0x1D 12 | #define ITG3200_BUFFER_SIZE 6 13 | #define ITG3200_RESET_ADDRESS 0x3E 14 | #define ITG3200_RESET_VALUE 0x80 15 | #define ITG3200_LOW_PASS_FILTER_ADDR 0x16 16 | #define ITG3200_LOW_PASS_FILTER_VALUE 0x1D // 10Hz low pass filter 17 | #define ITG3200_OSCILLATOR_ADDR 0x3E 18 | #define ITG3200_OSCILLATOR_VALUE 0x01 // use X gyro oscillator 19 | #define ITG3200_SCALE_TO_RADIANS 823.626831 // 14.375 LSBs per °/sec, / Pi / 180 20 | #define ITG3200_TEMPERATURE_ADDRESS 0x1B 21 | 22 | float gyro[3]; 23 | 24 | class ITG3200 { 25 | public: 26 | // Constructor 27 | ITG3200() { 28 | gyroScaleFactor = radians(1.0 / 14.375); // ITG3200 14.375 LSBs per °/sec 29 | 30 | gyroSamples = 0; 31 | }; 32 | 33 | void initialize() { 34 | // Check if sensor is alive 35 | Wire.beginTransmission(ITG3200_ADDRESS); 36 | Wire.write(0x00); 37 | Wire.endTransmission(); 38 | 39 | Wire.requestFrom(ITG3200_ADDRESS, 1); 40 | 41 | uint8_t register_value = Wire.read(); 42 | 43 | if ((register_value & ITG3200_IDENTITY_MASK) == ITG3200_IDENTITY) { 44 | sensors.sensors_detected |= GYROSCOPE_DETECTED; 45 | } else { 46 | return; 47 | } 48 | 49 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_RESET_ADDRESS, ITG3200_RESET_VALUE); // send a reset to the device 50 | 51 | // Startup delay 52 | delay(100); 53 | 54 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_LOW_PASS_FILTER_ADDR, ITG3200_LOW_PASS_FILTER_VALUE); // 10Hz low pass filter 55 | sensors.i2c_write8(ITG3200_ADDRESS, ITG3200_RESET_ADDRESS, ITG3200_OSCILLATOR_VALUE); // use internal oscillator 56 | 57 | // Let the gyro heat up 58 | delay(1500); 59 | 60 | // setup axis mapping 61 | if (CONFIG.data.GYRO_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 62 | CONFIG.data.GYRO_AXIS_MAP.axis1 = 1; // y 63 | CONFIG.data.GYRO_AXIS_MAP.axis2 = 0; // x 64 | CONFIG.data.GYRO_AXIS_MAP.axis3 = 2; // z 65 | 66 | CONFIG.data.GYRO_AXIS_MAP.axis1_sign = 0; 67 | CONFIG.data.GYRO_AXIS_MAP.axis2_sign = 0; 68 | CONFIG.data.GYRO_AXIS_MAP.axis3_sign = 0; 69 | 70 | CONFIG.data.GYRO_AXIS_MAP.initialized = 1; 71 | 72 | // save default in eeprom 73 | writeEEPROM(); 74 | } 75 | }; 76 | 77 | // ~1280ms 78 | void calibrate_gyro() { 79 | static uint8_t retry = 0; 80 | uint8_t i, count = 128; 81 | int16_t xSum = 0, ySum = 0, zSum = 0; 82 | 83 | for(i = 0; i < count; i++) { 84 | readGyroRaw(); 85 | xSum += gyroRaw[XAXIS]; 86 | ySum += gyroRaw[YAXIS]; 87 | zSum += gyroRaw[ZAXIS]; 88 | delay(10); 89 | } 90 | 91 | gyro_offset[XAXIS] = -xSum / count; 92 | gyro_offset[YAXIS] = -ySum / count; 93 | gyro_offset[ZAXIS] = -zSum / count; 94 | 95 | // Calibration sanity check 96 | // if suitable offset couldn't be established, break out of the loop after 10 retries 97 | if ((abs(gyro_offset[XAXIS]) > 140 || abs(gyro_offset[YAXIS]) > 140 || abs(gyro_offset[ZAXIS]) > 140) && retry < 10) { 98 | // gyro calibration failed, run again 99 | retry++; 100 | 101 | // small delay before next gyro calibration 102 | delay(500); 103 | 104 | calibrate_gyro(); 105 | } 106 | }; 107 | 108 | void readGyroRaw() { 109 | Wire.beginTransmission(ITG3200_ADDRESS); 110 | Wire.write(ITG3200_MEMORY_ADDRESS); 111 | Wire.endTransmission(); 112 | 113 | Wire.requestFrom(ITG3200_ADDRESS, ITG3200_BUFFER_SIZE); 114 | 115 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis1] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis1_sign?-1:1); 116 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis2] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis2_sign?-1:1); 117 | gyroRaw[CONFIG.data.GYRO_AXIS_MAP.axis3] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.GYRO_AXIS_MAP.axis3_sign?-1:1); 118 | }; 119 | 120 | void readGyroSum() { 121 | readGyroRaw(); 122 | 123 | gyroSum[XAXIS] += gyroRaw[XAXIS]; 124 | gyroSum[YAXIS] += gyroRaw[YAXIS]; 125 | gyroSum[ZAXIS] += gyroRaw[ZAXIS]; 126 | 127 | gyroSamples++; 128 | }; 129 | 130 | void evaluateGyro() { 131 | // Calculate average 132 | gyro[XAXIS] = gyroSum[XAXIS] / gyroSamples; 133 | gyro[YAXIS] = gyroSum[YAXIS] / gyroSamples; 134 | gyro[ZAXIS] = gyroSum[ZAXIS] / gyroSamples; 135 | 136 | // Apply offsets 137 | gyro[XAXIS] += gyro_offset[XAXIS]; 138 | gyro[YAXIS] += gyro_offset[YAXIS]; 139 | gyro[ZAXIS] += gyro_offset[ZAXIS]; 140 | 141 | // Apply correct scaling (at this point gyro is in radians) 142 | gyro[XAXIS] *= gyroScaleFactor; 143 | gyro[YAXIS] *= gyroScaleFactor; 144 | gyro[ZAXIS] *= gyroScaleFactor; 145 | 146 | // Reset SUM variables 147 | gyroSum[XAXIS] = 0; 148 | gyroSum[YAXIS] = 0; 149 | gyroSum[ZAXIS] = 0; 150 | gyroSamples = 0; 151 | }; 152 | private: 153 | int16_t gyro_offset[3]; 154 | int16_t gyroRaw[3]; 155 | float gyroSum[3]; 156 | float gyroScaleFactor; 157 | 158 | uint8_t gyroSamples; 159 | }; 160 | 161 | ITG3200 itg; 162 | 163 | void SensorArray::initializeGyro() { 164 | itg.initialize(); 165 | itg.calibrate_gyro(); 166 | } 167 | 168 | void SensorArray::readGyroSum() { 169 | itg.readGyroSum(); 170 | } 171 | 172 | void SensorArray::evaluateGyro() { 173 | itg.evaluateGyro(); 174 | } -------------------------------------------------------------------------------- /libraries/P_Kinematics/kinematics_ARG.h: -------------------------------------------------------------------------------- 1 | // Quaternion implementation of the 'DCM filter' [Mayhony et al]. 2 | // 3 | // This is the EXACT ARG kinematics taken from aeroquad flight controll software 4 | // @see: https://github.com/AeroQuad/AeroQuad 5 | // 6 | // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. 7 | // 8 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated 9 | // orientation. 10 | // 11 | // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') 12 | // and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer 13 | // units are irrelevant as the vector is normalised. 14 | 15 | float q0 = 1.0; 16 | float q1 = 0.0; 17 | float q2 = 0.0; 18 | float q3 = 0.0; 19 | float exInt = 0.0; 20 | float eyInt = 0.0; 21 | float ezInt = 0.0; 22 | 23 | float previousEx = 0; 24 | float previousEy = 0; 25 | float previousEz = 0; 26 | 27 | float Kp = 0.2; 28 | float Ki = 0.0005; 29 | 30 | unsigned long kinematics_timer; 31 | 32 | boolean isSwitched(float previousError, float currentError) { 33 | if ( (previousError > 0 && currentError < 0) || (previousError < 0 && currentError > 0)) { 34 | return true; 35 | } 36 | return false; 37 | } 38 | 39 | void argUpdate(float gx, float gy, float gz, float ax, float ay, float az, float G_Dt) { 40 | 41 | float norm; 42 | float vx, vy, vz; 43 | float q0i, q1i, q2i, q3i; 44 | float ex, ey, ez; 45 | 46 | float halfT = G_Dt / 2; 47 | 48 | // normalise the measurements 49 | norm = sqrt(ax * ax + ay * ay + az * az); 50 | ax = ax / norm; 51 | ay = ay / norm; 52 | az = az / norm; 53 | 54 | // estimated direction of gravity and flux (v and w) 55 | vx = 2 * (q1 * q3 - q0 * q2); 56 | vy = 2 * (q0 * q1 + q2 * q3); 57 | vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; 58 | 59 | // error is sum of cross product between reference direction of fields and direction measured by sensors 60 | ex = (vy * az - vz * ay); 61 | ey = (vz * ax - vx * az); 62 | ez = (vx * ay - vy * ax); 63 | 64 | // integral error scaled integral gain 65 | exInt = exInt + ex * Ki; 66 | if (isSwitched(previousEx,ex)) { 67 | exInt = 0.0; 68 | } 69 | previousEx = ex; 70 | 71 | eyInt = eyInt + ey * Ki; 72 | if (isSwitched(previousEy,ey)) { 73 | eyInt = 0.0; 74 | } 75 | previousEy = ey; 76 | 77 | ezInt = ezInt + ez * Ki; 78 | if (isSwitched(previousEz,ez)) { 79 | ezInt = 0.0; 80 | } 81 | previousEz = ez; 82 | 83 | // adjusted gyroscope measurements 84 | gx = gx + Kp * ex + exInt; 85 | gy = gy + Kp * ey + eyInt; 86 | gz = gz + Kp * ez + ezInt; 87 | 88 | // integrate quaternion rate and normalise 89 | q0i = (-q1 * gx - q2 * gy - q3 * gz) * halfT; 90 | q1i = ( q0 * gx + q2 * gz - q3 * gy) * halfT; 91 | q2i = ( q0 * gy - q1 * gz + q3 * gx) * halfT; 92 | q3i = ( q0 * gz + q1 * gy - q2 * gx) * halfT; 93 | q0 += q0i; 94 | q1 += q1i; 95 | q2 += q2i; 96 | q3 += q3i; 97 | 98 | // normalise quaternion 99 | norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 100 | q0 = q0 / norm; 101 | q1 = q1 / norm; 102 | q2 = q2 / norm; 103 | q3 = q3 / norm; 104 | } 105 | 106 | void kinematics_update(float gyroX, float gyroY, float gyroZ, float accelX, float accelY, float accelZ) { 107 | // Change signs on some of the variables 108 | float accelXfloat = accelX; 109 | float accelYfloat = -accelY; 110 | float accelZfloat = -accelZ; 111 | 112 | // Store current time 113 | float now = micros(); 114 | 115 | argUpdate(gyroX, gyroY, gyroZ, accelXfloat, accelYfloat, accelZfloat, (now - kinematics_timer) / 1000000.0); 116 | 117 | // Save kinematics time for next comparison 118 | kinematics_timer = now; 119 | 120 | kinematicsAngle[XAXIS] = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)); 121 | kinematicsAngle[YAXIS] = asin(2 * (q0 * q2 - q1 * q3)); 122 | kinematicsAngle[ZAXIS] = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)); 123 | } -------------------------------------------------------------------------------- /libraries/P_Kinematics/kinematics_CMP.h: -------------------------------------------------------------------------------- 1 | /* 2 | Kinematics implementation using first order complementary filter by cTn 3 | 4 | Extra: 5 | 1. accelerometer data normalization 6 | 2. accelerometer data cut-off 7 | 3. kinematics-gyroscope angle overflow handling 8 | 9 | Kinematics input is averaged (from multiple samples) and scaled (gyro is in radians/s) and accel is in m/s^2 10 | accel measurement is normalized before any angles are computed. 11 | */ 12 | 13 | unsigned long kinematics_timer; 14 | 15 | void kinematics_update(float gyroX, float gyroY, float gyroZ, float accelX, float accelY, float accelZ) { 16 | 17 | // Normalize accel values 18 | float norm = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ); 19 | accelX /= norm; 20 | accelY /= norm; 21 | accelZ /= norm; 22 | 23 | float accelXangle = atan2(accelY, accelZ); 24 | float accelYangle = atan2(accelX, accelZ); 25 | //float accelYangle = atan2(*accelX, sqrt(*accelY * *accelY + *accelZ * *accelZ)); 26 | 27 | // Accelerometer cut-off 28 | float accelWeight = 0.0050; // normal operation 29 | if (norm > 13.0 || norm < 7.0) accelWeight = 0.00; // gyro only 30 | 31 | // Save current time into variable for better computation time 32 | unsigned long now = micros(); 33 | 34 | // Fuse in gyroscope 35 | kinematicsAngle[XAXIS] += (gyroX * (float)(now - kinematics_timer) / 1000000); 36 | kinematicsAngle[YAXIS] += (gyroY * (float)(now - kinematics_timer) / 1000000); 37 | kinematicsAngle[ZAXIS] += (gyroZ * (float)(now - kinematics_timer) / 1000000); 38 | 39 | // Normalize gyro kinematics (+- PI) 40 | NORMALIZE(kinematicsAngle[XAXIS]); 41 | NORMALIZE(kinematicsAngle[YAXIS]); 42 | NORMALIZE(kinematicsAngle[ZAXIS]); 43 | 44 | // Fuse in accel 45 | // This is second order accelerometer cut off, which restricts accel data fusion in only 46 | // "up-side UP" orientation and restricts it further to avoid incorrect accelerometer data fusion. 47 | if (accelZ > 0.75) { 48 | if ((kinematicsAngle[XAXIS] - accelXangle) > PI) { 49 | kinematicsAngle[XAXIS] = (1.00 - accelWeight) * kinematicsAngle[XAXIS] + accelWeight * (accelXangle + TWO_PI); 50 | } else if ((kinematicsAngle[XAXIS] - accelXangle) < -PI) { 51 | kinematicsAngle[XAXIS] = (1.00 - accelWeight) * kinematicsAngle[XAXIS] + accelWeight * (accelXangle - TWO_PI); 52 | } else { 53 | kinematicsAngle[XAXIS] = (1.00 - accelWeight) * kinematicsAngle[XAXIS] + accelWeight * accelXangle; 54 | } 55 | } 56 | 57 | if (accelZ > 0.60) { 58 | if ((kinematicsAngle[YAXIS] - accelYangle) > PI) { 59 | kinematicsAngle[YAXIS] = (1.00 - accelWeight) * kinematicsAngle[YAXIS] + accelWeight * (accelYangle + TWO_PI); 60 | } else if ((kinematicsAngle[YAXIS] - accelYangle) < -PI) { 61 | kinematicsAngle[YAXIS] = (1.00 - accelWeight) * kinematicsAngle[YAXIS] + accelWeight * (accelYangle - TWO_PI); 62 | } else { 63 | kinematicsAngle[YAXIS] = (1.00 - accelWeight) * kinematicsAngle[YAXIS] + accelWeight * accelYangle; 64 | } 65 | } 66 | // Saves time for next comparison 67 | kinematics_timer = now; 68 | } -------------------------------------------------------------------------------- /libraries/P_Kinematics/kinematics_DCM.h: -------------------------------------------------------------------------------- 1 | /* 2 | Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion 3 | compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference 4 | direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw axis only. 5 | 6 | This is the EXACT AHRS algorythm used in Free IMU, developed by Fabio Varesano (deceased) =( 7 | */ 8 | 9 | #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain 10 | #define twoKiDef (2.0f * 0.1f) // 2 * integral gain 11 | 12 | // initialize quaternion 13 | float q0 = 1.0f; 14 | float q1 = 0.0f; 15 | float q2 = 0.0f; 16 | float q3 = 0.0f; 17 | float exInt = 0.0; 18 | float eyInt = 0.0; 19 | float ezInt = 0.0; 20 | float twoKp = twoKpDef; 21 | float twoKi = twoKiDef; 22 | float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; 23 | float sampleFreq; 24 | unsigned long kinematics_timer = 0; 25 | 26 | void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) { 27 | float recipNorm; 28 | float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; 29 | float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; 30 | float qa, qb, qc; 31 | 32 | // Auxiliary variables to avoid repeated arithmetic 33 | q0q0 = q0 * q0; 34 | q0q1 = q0 * q1; 35 | q0q2 = q0 * q2; 36 | q0q3 = q0 * q3; 37 | q1q1 = q1 * q1; 38 | q1q2 = q1 * q2; 39 | q1q3 = q1 * q3; 40 | q2q2 = q2 * q2; 41 | q2q3 = q2 * q3; 42 | q3q3 = q3 * q3; 43 | 44 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 45 | if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) { 46 | float halfvx, halfvy, halfvz; 47 | 48 | // Normalise accelerometer measurement 49 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 50 | ax *= recipNorm; 51 | ay *= recipNorm; 52 | az *= recipNorm; 53 | 54 | // Estimated direction of gravity 55 | halfvx = q1q3 - q0q2; 56 | halfvy = q0q1 + q2q3; 57 | halfvz = q0q0 - 0.5f + q3q3; 58 | 59 | // Error is sum of cross product between estimated direction and measured direction of field vectors 60 | halfex += (ay * halfvz - az * halfvy); 61 | halfey += (az * halfvx - ax * halfvz); 62 | halfez += (ax * halfvy - ay * halfvx); 63 | } 64 | 65 | // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer 66 | if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { 67 | // Compute and apply integral feedback if enabled 68 | if(twoKi > 0.0f) { 69 | integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki 70 | integralFBy += twoKi * halfey * (1.0f / sampleFreq); 71 | integralFBz += twoKi * halfez * (1.0f / sampleFreq); 72 | gx += integralFBx; // apply integral feedback 73 | gy += integralFBy; 74 | gz += integralFBz; 75 | } 76 | else { 77 | integralFBx = 0.0f; // prevent integral windup 78 | integralFBy = 0.0f; 79 | integralFBz = 0.0f; 80 | } 81 | 82 | // Apply proportional feedback 83 | gx += twoKp * halfex; 84 | gy += twoKp * halfey; 85 | gz += twoKp * halfez; 86 | } 87 | 88 | // Integrate rate of change of quaternion 89 | gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors 90 | gy *= (0.5f * (1.0f / sampleFreq)); 91 | gz *= (0.5f * (1.0f / sampleFreq)); 92 | qa = q0; 93 | qb = q1; 94 | qc = q2; 95 | q0 += (-qb * gx - qc * gy - q3 * gz); 96 | q1 += (qa * gx + qc * gz - q3 * gy); 97 | q2 += (qa * gy - qb * gz + q3 * gx); 98 | q3 += (qa * gz + qb * gy - qc * gx); 99 | 100 | // Normalise quaternion 101 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 102 | q0 *= recipNorm; 103 | q1 *= recipNorm; 104 | q2 *= recipNorm; 105 | q3 *= recipNorm; 106 | } 107 | 108 | 109 | void kinematics_update(float gyroX, float gyroY, float gyroZ, float accelX, float accelY, float accelZ) { 110 | unsigned long now = micros(); 111 | sampleFreq = 1.0 / ((now - kinematics_timer) / 1000000.0); 112 | kinematics_timer = now; 113 | 114 | // Some sensors require different orientation, do the inverse here. 115 | // Also store sensor data in different variables as they are also used elsewhere 116 | float gyX = gyroX; 117 | float gyY = -gyroY; 118 | float gyZ = gyroZ; 119 | float acX = accelX; 120 | float acY = accelY; 121 | float acZ = accelZ; 122 | 123 | AHRSupdate(gyX, gyY, gyZ, acX, acY, acZ); 124 | 125 | float gx = 2 * (q1 * q3 - q0 * q2); 126 | float gy = 2 * (q0 * q1 + q2 * q3); 127 | float gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; 128 | 129 | kinematicsAngle[XAXIS] = atan(gy / sqrt(gx * gx + gz * gz)); 130 | kinematicsAngle[YAXIS] = atan(gx / sqrt(gy * gy + gz * gz)); 131 | kinematicsAngle[ZAXIS] = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 - 1); 132 | 133 | // invert 134 | kinematicsAngle[ZAXIS] = -kinematicsAngle[ZAXIS]; 135 | } -------------------------------------------------------------------------------- /libraries/P_Magnetometer/Magnetometer_HMC5883L.h: -------------------------------------------------------------------------------- 1 | /* HMC5883L magnetometer implementation 2 | 3 | Featuring accelerometer tilt compensation, for more information visit 4 | @ https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial 5 | 6 | Supporting 360 deg heading (+- 180deg), to use with 0-360 deg kinematics add + PI 7 | 8 | Output inside absolute heading is in radians. 9 | */ 10 | 11 | #define HMC5883L_ADDRESS 0x1E 12 | 13 | #define HMC5883L_RA_CONFIG_A 0x00 14 | #define HMC5883L_RA_CONFIG_B 0x01 15 | #define HMC5883L_RA_MODE 0x02 16 | 17 | #define HMC5883L_MODE_CONTINUOUS 0x00 18 | #define HMC5883L_MODE_SINGLE 0x01 19 | #define HMC5883L_MODE_IDLE 0x02 20 | 21 | #define HMC5883L_GAIN_07 0x00 // +- 0.7 Ga 22 | #define HMC5883L_GAIN_10 0x20 // +- 1.0 Ga (default) 23 | #define HMC5883L_GAIN_15 0x40 // +- 1.5 Ga 24 | #define HMC5883L_GAIN_20 0x60 // +- 2.0 Ga 25 | #define HMC5883L_GAIN_32 0x80 // +- 3.2 Ga 26 | #define HMC5883L_GAIN_38 0xA0 // +- 3.8 Ga 27 | #define HMC5883L_GAIN_45 0xC0 // +- 4.5 Ga 28 | #define HMC5883L_GAIN_65 0xE0 // +- 6.5 Ga 29 | 30 | int16_t magRaw[3]; 31 | float magHeadingX, magHeadingY; 32 | float magHeadingAbsolute = 0.0; 33 | 34 | class HMC5883L { 35 | public: 36 | // Constructor 37 | HMC5883L() { 38 | }; 39 | 40 | void initialize() { 41 | // Check if sensor is alive 42 | Wire.beginTransmission(HMC5883L_ADDRESS); 43 | Wire.write(0x00); 44 | Wire.endTransmission(); 45 | 46 | Wire.requestFrom(HMC5883L_ADDRESS, 1); 47 | 48 | uint8_t register_value = Wire.read(); 49 | 50 | if (register_value == 0x10) { 51 | sensors.sensors_detected |= MAGNETOMETER_DETECTED; 52 | } else { 53 | return; 54 | } 55 | 56 | // Set gain to +- 1.0 Ga 57 | sensors.i2c_write8(HMC5883L_ADDRESS, HMC5883L_RA_CONFIG_B, HMC5883L_GAIN_10); 58 | 59 | // Start in single mode 60 | sensors.i2c_write8(HMC5883L_ADDRESS, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE); 61 | 62 | // setup axis mapping 63 | if (CONFIG.data.MAG_AXIS_MAP.initialized == 0) { // check if map was defined before, if not "save default" order set 64 | CONFIG.data.MAG_AXIS_MAP.axis1 = 0; // x 65 | CONFIG.data.MAG_AXIS_MAP.axis2 = 2; // z 66 | CONFIG.data.MAG_AXIS_MAP.axis3 = 1; // y 67 | 68 | CONFIG.data.MAG_AXIS_MAP.axis1_sign = 0; 69 | CONFIG.data.MAG_AXIS_MAP.axis2_sign = 0; 70 | CONFIG.data.MAG_AXIS_MAP.axis3_sign = 0; 71 | 72 | CONFIG.data.MAG_AXIS_MAP.initialized = 1; 73 | 74 | // save default in eeprom 75 | writeEEPROM(); 76 | } 77 | }; 78 | 79 | void readMagRaw() { 80 | Wire.beginTransmission(HMC5883L_ADDRESS); 81 | Wire.write(0x03); 82 | Wire.endTransmission(); 83 | 84 | Wire.requestFrom(HMC5883L_ADDRESS, 6); 85 | 86 | magRaw[CONFIG.data.MAG_AXIS_MAP.axis1] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.MAG_AXIS_MAP.axis1_sign?-1:1); 87 | magRaw[CONFIG.data.MAG_AXIS_MAP.axis2] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.MAG_AXIS_MAP.axis2_sign?-1:1); 88 | magRaw[CONFIG.data.MAG_AXIS_MAP.axis3] = ((Wire.read() << 8) | Wire.read()) * (CONFIG.data.MAG_AXIS_MAP.axis3_sign?-1:1); 89 | 90 | // start single conversion 91 | sensors.i2c_write8(HMC5883L_ADDRESS, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE); 92 | }; 93 | 94 | void evaluateMag() { 95 | float cosRoll = cos(kinematicsAngle[XAXIS]); 96 | float sinRoll = sin(kinematicsAngle[XAXIS]); 97 | float cosPitch = cos(kinematicsAngle[YAXIS]); 98 | float sinPitch = sin(kinematicsAngle[YAXIS]); 99 | 100 | // Apply accelerometer tilt corrections 101 | magX = magRaw[XAXIS] * cosPitch + magRaw[ZAXIS] * sinPitch; 102 | magY = magRaw[XAXIS] * sinRoll * sinPitch + magRaw[YAXIS] * cosRoll - magRaw[ZAXIS] * sinRoll * cosPitch; 103 | 104 | // Normalize measurements 105 | float norm = sqrt(magX * magX + magY * magY); 106 | 107 | magHeadingX = magX / norm; 108 | magHeadingY = -magY / norm; 109 | 110 | // Calculate absolute heading 111 | magHeadingAbsolute = atan2(magHeadingY, magHeadingX); 112 | }; 113 | 114 | private: 115 | float magX, magY; 116 | }; 117 | 118 | HMC5883L hmc5883l; 119 | 120 | void SensorArray::initializeMag() { 121 | hmc5883l.initialize(); 122 | } 123 | 124 | void SensorArray::readMag() { 125 | hmc5883l.readMagRaw(); 126 | } 127 | 128 | void SensorArray::evaluateMag() { 129 | hmc5883l.evaluateMag(); 130 | } -------------------------------------------------------------------------------- /libraries/P_Receiver/Receiver_328p_HW_PPM.h: -------------------------------------------------------------------------------- 1 | /* PPM (pulse position modulation) sampling done in hardware via timer1. 2 | Sampling is done via digital pin 8. 3 | 4 | Code below supports frame buffering, which is used to protect the controller from 5 | errors introduced (from simple noise to complete RX failure), if any of the channels in a single 6 | PPM frame triggered the sanity check, whole frame will be droped and PPM signal values will remain 7 | unchanged until a valid frame can be sampled. 8 | 9 | PPM_error variable will be increased every time a corrupted frame was detected. 10 | 11 | RX_signalReceived flag is set to 0 every time a valid frame is processed, this variables is used to 12 | "tell" software there was an error in communication (anything from a corrupted frame to a complete 13 | receiver failure), this flag is increased by 1 every 10ms, if it reaches 100 (represnting 1 second) 14 | an subroutine is triggered that should handle this failure condition, from the most simple routines 15 | that will just disarm the craft (making it fall from the sky like a boiled potato) to more adnvaced 16 | auto-descent routines. 17 | 18 | Please remember that this failsafe is here to prevent craft from "flying away on its own" and 19 | potentionaly harming someone in the proces (damaged craft from a crash is still better then cutting 20 | someones head off with uncontrollable craft). 21 | 22 | Big thanks to kha from #aeroquad for setting up the shared timer. 23 | */ 24 | #define RX_PPM_SYNCPULSE 5000 // 2.5ms 25 | 26 | #define RX_CHANNELS 16 // dont change this 27 | volatile uint16_t RX[RX_CHANNELS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; 28 | volatile uint16_t PPM_temp[RX_CHANNELS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Temporary buffer 29 | 30 | volatile uint16_t startPulse = 0; 31 | volatile uint8_t ppmCounter = RX_CHANNELS; 32 | volatile uint16_t PPM_error = 0; 33 | 34 | volatile uint8_t RX_signalReceived = 0; 35 | 36 | bool failsafeEnabled = false; 37 | 38 | #define TIMER1_FREQUENCY_HZ 50 39 | #define TIMER1_PRESCALER 8 40 | #define TIMER1_PERIOD (F_CPU/TIMER1_PRESCALER/TIMER1_FREQUENCY_HZ) 41 | 42 | // Capture Interrupt Vector 43 | ISR(TIMER1_CAPT_vect) { 44 | unsigned int stopPulse = ICR1; 45 | unsigned int pulseWidth = stopPulse - startPulse; 46 | 47 | // Error / Sanity check 48 | // if pulseWidth < 900us or pulseWidth > 2100us and pulseWidth < RX_PPM_SYNCPULSE 49 | if (pulseWidth < 1800 || (pulseWidth > 4200 && pulseWidth < RX_PPM_SYNCPULSE)) { 50 | PPM_error++; 51 | 52 | // set ppmCounter out of range so rest and (later on) whole frame is dropped 53 | ppmCounter = RX_CHANNELS + 1; 54 | } 55 | 56 | if (pulseWidth >= RX_PPM_SYNCPULSE) { // Verify if this is the sync pulse 57 | if (ppmCounter <= RX_CHANNELS) { 58 | // This indicates that we received an correct frame = push to the "main" PPM array 59 | // if we received an broken frame, it will get ignored here and later get over-written 60 | // by new data, that will also be checked for sanity. 61 | for (uint8_t i = 0; i < RX_CHANNELS; i++) { 62 | RX[i] = PPM_temp[i]; 63 | } 64 | 65 | // Bring failsafe flag down every time we accept a valid signal / frame 66 | RX_signalReceived = 0; 67 | } 68 | ppmCounter = 0; // restart the channel counter 69 | } else { 70 | if (ppmCounter < RX_CHANNELS) { // extra RX_CHANNELS will get ignored here 71 | PPM_temp[ppmCounter] = pulseWidth / 2; // Store measured pulse length in us 72 | ppmCounter++; // Advance to next channel 73 | } 74 | } 75 | 76 | startPulse = stopPulse; // Save time at pulse start 77 | } 78 | 79 | // Timer1 setup (shared timer for PPM sampling & PWM servo signal generation) 80 | void setupTimer1RX() { 81 | // Setup timer1 in normal mode, count at 2MHz 82 | TCCR1A = 0; 83 | TCCR1B = (1 << CS11) | (1 << ICES1); 84 | TIMSK1 |= (1 << ICIE1) | (1 << OCIE1A); // Enable ICP and OCRA interrupts 85 | } 86 | 87 | void initializeReceiver() { 88 | setupTimer1RX(); 89 | } 90 | 91 | void RX_failSafe() { 92 | RX_signalReceived++; // if this flag reaches 10, an auto-descent routine will be triggered. 93 | 94 | if (RX_signalReceived > 10) { 95 | RX_signalReceived = 10; // don't let the variable overflow 96 | failsafeEnabled = true; // this ensures that failsafe will operate in attitude mode 97 | 98 | // Bear in mind that this is here just to "slow" the fall, if you have lets say 500m altitude, 99 | // this probably won't help you much (sorry). 100 | // This will slowly (-2 every 100ms) bring the throttle to 1000 (still saved in the PPM array) 101 | // 1000 = 0 throttle; 102 | // Descending from FULL throttle 2000 (most unlikely) would take about 1 minute and 40 seconds 103 | // Descending from HALF throttle 1500 (more likely) would take about 50 seconds 104 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] -= 2; 105 | 106 | if (RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] < 1000) { 107 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] = 1000; // don't let the value fall below 1000 108 | } 109 | } else { 110 | failsafeEnabled = false; 111 | } 112 | } -------------------------------------------------------------------------------- /libraries/P_Receiver/Receiver_teensy3_HW_PPM.h: -------------------------------------------------------------------------------- 1 | /* PPM (pulse position modulation) sampling done in hardware via FLEX timer. 2 | 3 | Signal sampling is being done via PORTA_PCR12 (PIN 3). 4 | 5 | We are using flex timer1 which supports only 2 channels. 6 | This code only utilizes single edge capture which is more then enough in terms of accuracy. 7 | 8 | Code below supports frame buffering, which is used to protect the controller from 9 | errors introduced (from simple noise to complete RX failure), if any of the channels in a single 10 | PPM frame triggered the sanity check, whole frame will be droped and PPM signal values will remain 11 | unchanged until a valid frame can be sampled. 12 | 13 | PPM_error variable will be increased every time a corrupted frame was detected. 14 | 15 | RX_signalReceived flag is set to 0 every time a valid frame is processed, this variables is used to 16 | "tell" software there was an error in communication (anything from a corrupted frame to a complete 17 | receiver failure), this flag is increased by 1 every 10ms, if it reaches 100 (represnting 1 second) 18 | an subroutine is triggered that should handle this failure condition, from the most simple routines 19 | that will just disarm the craft (making it fall from the sky like a boiled potato) to more adnvaced 20 | auto-descent routines. 21 | 22 | Please remember that this failsafe is here to prevent craft from "flying away on its own" and 23 | potentionaly harming someone in the proces (damaged craft from a crash is still better then cutting 24 | someones head off with uncontrollable craft). 25 | 26 | Big thanks to kha from #aeroquad and Ragnorok from #arduino for helping me get this up and running. 27 | 28 | TODO: Sync pulse length varies among receivers, should probably provide a "define" style table for defining the 29 | sync pulse length. 30 | */ 31 | #define RX_PPM_SYNCPULSE 7500 // 2.5ms 32 | 33 | #define RX_CHANNELS 16 34 | volatile uint16_t RX[RX_CHANNELS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; 35 | volatile uint16_t PPM_temp[RX_CHANNELS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Temporary buffer 36 | 37 | volatile uint16_t startPulse = 0; 38 | volatile uint8_t ppmCounter = RX_CHANNELS; 39 | volatile uint16_t PPM_error = 0; 40 | 41 | volatile uint8_t RX_signalReceived = 0; 42 | 43 | bool failsafeEnabled = false; 44 | 45 | extern "C" void ftm1_isr(void) { 46 | // save current interrupt count/time 47 | uint16_t stopPulse = FTM1_C0V; 48 | 49 | // clear channel interrupt flag (CHF) 50 | FTM1_C0SC &= ~0x80; 51 | 52 | uint16_t pulseWidth = stopPulse - startPulse; 53 | 54 | // Error / Sanity check 55 | // if pulseWidth < 900us or pulseWidth > 2100us and pulseWidth < RX_PPM_SYNCPULSE 56 | if (pulseWidth < 2700 || (pulseWidth > 6300 && pulseWidth < RX_PPM_SYNCPULSE)) { 57 | PPM_error++; 58 | 59 | // set ppmCounter out of range so rest and (later on) whole frame is dropped 60 | ppmCounter = RX_CHANNELS + 1; 61 | } 62 | 63 | if (pulseWidth >= RX_PPM_SYNCPULSE) { // Verify if this is the sync pulse 64 | if (ppmCounter <= RX_CHANNELS) { 65 | // This indicates that we received an correct frame = push to the "main" PPM array 66 | // if we received an broken frame, it will get ignored here and later get over-written 67 | // by new data, that will also be checked for sanity. 68 | 69 | for (uint8_t i = 0; i < RX_CHANNELS; i++) { 70 | RX[i] = PPM_temp[i]; 71 | } 72 | 73 | // Bring failsafe flag down every time we accept a valid signal / frame 74 | RX_signalReceived = 0; 75 | } 76 | 77 | // restart the channel counter 78 | ppmCounter = 0; 79 | } else { 80 | if (ppmCounter < RX_CHANNELS) { // extra channels will get ignored here 81 | PPM_temp[ppmCounter] = pulseWidth / 3; // Store measured pulse length in us 82 | ppmCounter++; // Advance to next channel 83 | } 84 | } 85 | 86 | startPulse = stopPulse; // Save time at pulse start 87 | } 88 | 89 | void setupFTM1() { 90 | // FLEX Timer1 input filter configuration 91 | // 4+4×val clock cycles, 48MHz = 4+4*7 = 32 clock cycles = 0.75us 92 | FTM1_FILTER = 0x07; 93 | 94 | // FLEX Timer1 configuration 95 | FTM1_SC = 0x0C; // TOF=0 TOIE=0 CPWMS=0 CLKS=01 (system clock) PS=100 (divide by 16) 96 | FTM1_MOD = 0xFFFF; // modulo to max 97 | FTM1_C0SC = 0x44; // CHF=0 CHIE=1 MSB=0 MSA=0 ELSB=0 ELSA=1 DMA=0 98 | 99 | // Enable FTM1 interrupt inside NVIC 100 | NVIC_ENABLE_IRQ(IRQ_FTM1); 101 | 102 | // PIN configuration, alternative function 3 103 | PORTA_PCR12 |= 0x300; 104 | } 105 | 106 | void initializeReceiver() { 107 | setupFTM1(); 108 | } 109 | 110 | void RX_failSafe() { 111 | RX_signalReceived++; // if this flag reaches 10, an auto-descent routine will be triggered. 112 | 113 | if (RX_signalReceived > 10) { 114 | RX_signalReceived = 10; // don't let the variable overflow 115 | failsafeEnabled = true; // this ensures that failsafe will operate in attitude mode 116 | 117 | // Bear in mind that this is here just to "slow" the fall, if you have lets say 500m altitude, 118 | // this probably won't help you much (sorry). 119 | // This will slowly (-2 every 100ms) bring the throttle to 1000 (still saved in the PPM array) 120 | // 1000 = 0 throttle; 121 | // Descending from FULL throttle 2000 (most unlikely) would take about 1 minute and 40 seconds 122 | // Descending from HALF throttle 1500 (more likely) would take about 50 seconds 123 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] -= 2; 124 | 125 | if (RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] < 1000) { 126 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] = 1000; // don't let the value fall below 1000 127 | } 128 | } else { 129 | failsafeEnabled = false; 130 | } 131 | } -------------------------------------------------------------------------------- /libraries/P_Receiver/Receiver_teensy3_HW_PWM.h: -------------------------------------------------------------------------------- 1 | /* PWM (Pulse Width Modulation) sampling done in hardware via pin interrupts and PIT timer. 2 | 3 | I am using one, out of 4 PIT timers build into the Teensy 3.0. 4 | 5 | Pins used for PWM signal capture are fully definable by the user. 6 | However, i do recommend avoiding FLEX timer enabled pins, (Teensy 3.0 pin numbering) 5, 6, 9, 10, 20, 21, 22, 23, 7 | as those are used for PWM signal generation (for Electronic Speed Controllers). 8 | 9 | Using the hardware timer for timing / counting the pulses gives us superior accuracy of ~21 nano seconds. 10 | 11 | This receiver code also utilizes a more advanced failsafe sequence. 12 | In case of receiver malfunction / signal cable damage, RX Failasefe will kicks in 13 | which will start auto-descent sequence. 14 | 15 | */ 16 | 17 | #define RX_CHANNELS 16 18 | uint8_t PWM_PINS[RX_CHANNELS] = {2, 3, 4, 5, 6, 7, 8, 9}; 19 | 20 | volatile uint16_t RX[RX_CHANNELS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; 21 | volatile uint32_t PWM_time[RX_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; 22 | 23 | volatile uint16_t RX_failsafeStatus; 24 | volatile uint8_t RX_signalReceived = 0; 25 | 26 | bool failsafeEnabled = false; 27 | 28 | void readPWM(uint8_t channel) { 29 | uint32_t now = PIT_CVAL0; // Current counter value 30 | uint32_t delta = PWM_time[channel] - now; // Delta (calculated in reverse, because PIT is downcounting timer) 31 | 32 | // All of the number below are scaled to use with Teensy 3.0 running at 48Mhz 33 | if (delta < 120000 && delta > 43200) { // This is a valid pulse 34 | RX[channel] = delta / 48; 35 | 36 | // Bring failsafe status flag for current channel down every time we accept a valid signal 37 | RX_failsafeStatus &= ~(1 << channel); 38 | } else { // Beginning of the pulse 39 | PWM_time[channel] = now; 40 | } 41 | } 42 | 43 | // ISRs 44 | void PWM_ISR_0() { 45 | readPWM(0); 46 | } 47 | void PWM_ISR_1() { 48 | readPWM(1); 49 | } 50 | void PWM_ISR_2() { 51 | readPWM(2); 52 | } 53 | void PWM_ISR_3() { 54 | readPWM(3); 55 | } 56 | void PWM_ISR_4() { 57 | readPWM(4); 58 | } 59 | void PWM_ISR_5() { 60 | readPWM(5); 61 | } 62 | void PWM_ISR_6() { 63 | readPWM(6); 64 | } 65 | void PWM_ISR_7() { 66 | readPWM(7); 67 | } 68 | 69 | void (*PWM_Handlers [])(void) = { 70 | PWM_ISR_0, 71 | PWM_ISR_1, 72 | PWM_ISR_2, 73 | PWM_ISR_3, 74 | PWM_ISR_4, 75 | PWM_ISR_5, 76 | PWM_ISR_6, 77 | PWM_ISR_7 78 | }; 79 | 80 | void initializeReceiver() { 81 | // initialize PIT timer (teensy running at 48000000) 82 | PIT_MCR = 0x00; // Turn on PIT 83 | PIT_LDVAL0 = 0xFFFFFFFF; // Load initial value of 4294967295 84 | PIT_TCTRL0 = 0x01; // Start the counter 85 | 86 | for (uint8_t i = 0; i < RX_CHANNELS; i++) { 87 | pinMode(PWM_PINS[i], INPUT); 88 | attachInterrupt(PWM_PINS[i], PWM_Handlers[i], CHANGE); 89 | } 90 | } 91 | 92 | void RX_failSafe() { 93 | if (RX_failsafeStatus > 0) { 94 | RX_signalReceived++; // if this flag reaches 10, an auto-descent routine will be triggered. 95 | } else { 96 | // Raise the FLAGS 97 | RX_failsafeStatus |= (1 << RX_CHANNELS) - 1; 98 | 99 | // Reset the counter 100 | RX_signalReceived = 0; 101 | } 102 | 103 | if (RX_signalReceived > 10) { 104 | RX_signalReceived = 10; // don't let the variable overflow 105 | failsafeEnabled = true; // this ensures that failsafe will operate in attitude mode 106 | 107 | // Bear in mind that this is here just to "slow" the fall, if you have lets say 500m altitude, 108 | // this probably won't help you much (sorry). 109 | // This will slowly (-2 every 100ms) bring the throttle to 1000 (still saved in the PPM array) 110 | // 1000 = 0 throttle; 111 | // Descending from FULL throttle 2000 (most unlikely) would take about 1 minute and 40 seconds 112 | // Descending from HALF throttle 1500 (more likely) would take about 50 seconds 113 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] -= 2; 114 | 115 | if (RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] < 1000) { 116 | RX[CONFIG.data.CHANNEL_ASSIGNMENT[THROTTLE]] = 1000; // don't let the value fall below 1000 117 | } 118 | } else { 119 | failsafeEnabled = false; 120 | } 121 | } -------------------------------------------------------------------------------- /libraries/P_Sonar/Sonar_srf04.h: -------------------------------------------------------------------------------- 1 | #define SONAR_RANGE 300 // 300cm 2 | #define SONAR_PIN_TRIGGER 15 3 | #define SONAR_PIN_ECHO 16 4 | 5 | unsigned long sonar_start = 0; 6 | unsigned long sonar_end = 0; 7 | bool sonar_ready = true; 8 | 9 | float sonar_raw = 0.0; 10 | float sonarAltitude = 0.0; 11 | float sonarAltitudeToHoldTarget = 0.0; 12 | int16_t sonarAltitudeHoldThrottle = 1000; 13 | 14 | void readSonar() { 15 | // SRF04 Requires at least 10us long TTL pulse to trigger 16 | // This pin will go low in next 1000 <= us 17 | digitalWrite(SONAR_PIN_TRIGGER, HIGH); 18 | } 19 | 20 | void readSonarFinish() { 21 | digitalWrite(SONAR_PIN_TRIGGER, LOW); 22 | } 23 | 24 | void sonarEcho() { 25 | if (sonar_ready) { 26 | sonar_start = micros(); 27 | sonar_ready = false; 28 | } else { 29 | // The speed of sound is 340 m/s or 29 microseconds per centimeter. 30 | // The ping travels out and back, so to find the distance of the 31 | // object we take half of the distance travelled. 32 | sonar_end = micros(); 33 | 34 | sonar_raw = sonar_end - sonar_start; 35 | sonar_raw = sonar_raw / 29.0 / 2.0; 36 | 37 | if (sonar_raw > SONAR_RANGE) { 38 | sonar_raw = 0; 39 | } else { 40 | // Compute 41 | // Convert centimeters to meters (to match baro units) 42 | sonar_raw /= 100; 43 | 44 | sonarAltitude = filterSmooth(sonar_raw, sonarAltitude, 0.75); 45 | } 46 | 47 | sonar_ready = true; 48 | } 49 | } 50 | 51 | void initializeSonar() { 52 | pinMode(SONAR_PIN_TRIGGER, OUTPUT); 53 | pinMode(SONAR_PIN_ECHO, INPUT); 54 | attachInterrupt(SONAR_PIN_ECHO, sonarEcho, CHANGE); 55 | } --------------------------------------------------------------------------------