├── DEPRECATED ├── BrushedFlightController-AltHold-Multiwii │ ├── BMX055.cpp │ ├── BMX055.h │ ├── BrushedFlightController-AltHold-Multiwii.ino │ ├── Estimates.cpp │ ├── Estimates.h │ ├── MS5611.cpp │ ├── MS5611.h │ └── i2c_wrapper.h ├── BrushedFlightController-RTOS-BMX055-CC1125-Multiwii │ ├── BMX055.cpp │ ├── BMX055.h │ ├── BrushedFlightController-RTOS-BMX055-CC1125-Multiwii.ino │ ├── CC1125.cpp │ ├── CC1125.h │ ├── RollingMemory.h │ ├── SensorFusion.cpp │ ├── SensorFusion.h │ └── i2c_wrapper.h └── BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii │ ├── BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii.ino │ ├── IMU.cpp │ ├── IMU.h │ ├── MPU.cpp │ ├── MPU.h │ └── i2c_wrapper.h ├── ESP32BrushedFlightController ├── BMX055.cpp ├── BMX055.h ├── Definitions.h ├── ESP32BrushedFlightController.ino ├── Estimates.cpp ├── Estimates.h ├── MS5611.cpp ├── MS5611.h ├── PID.cpp ├── PID.h └── i2c_wrapper.h ├── FlowDiagram-RTOS.jpg ├── HWRemoteControl ├── DEPRECATED │ ├── ArduinoPS2_SimpleTest │ │ ├── ArduinoPS2_SimpleTest.ino │ │ ├── PS2X_lib.cpp │ │ └── PS2X_lib.h │ ├── ESP32PS2_SimpleTest │ │ ├── ESP32PS2_SimpleTest.ino │ │ ├── PsxLib.cpp │ │ └── PsxLib.h │ ├── JoystickTests │ │ └── JoystickTests.ino │ ├── PS2X_Example │ │ ├── PS2X_Example.ino │ │ ├── PS2X_lib.cpp │ │ └── PS2X_lib.h │ ├── PS2_CC1125_Receive │ │ ├── CC1125.cpp │ │ ├── CC1125.h │ │ └── PS2_CC1125_Receive.ino │ ├── PS2_CC1125_Transmit │ │ ├── CC1125.cpp │ │ ├── CC1125.h │ │ ├── PS2_CC1125_Transmit.ino │ │ ├── PsxLib.cpp │ │ └── PsxLib.h │ ├── PS2_ESPNOW_Receive │ │ └── PS2_ESPNOW_Receive.ino │ ├── PS2_NRFReceive │ │ ├── PS2_NRFReceive.ino │ │ ├── RF24.cpp │ │ ├── RF24.h │ │ ├── RF24_config.h │ │ ├── nRF24L01.h │ │ └── printf.h │ └── PS2_NRFTransmit │ │ ├── PS2_NRFTransmit.ino │ │ ├── PsxLib.cpp │ │ ├── PsxLib.h │ │ ├── RF24.cpp │ │ ├── RF24.h │ │ ├── RF24_config.h │ │ ├── nRF24L01.h │ │ └── printf.h ├── ESPNOW_Telemetry │ └── ESPNOW_Telemetry.ino └── PS2_ESPNOW_Transmit │ ├── PS2_ESPNOW_Transmit.ino │ ├── PsxLib.cpp │ └── PsxLib.h ├── LICENSE ├── PIDSimulationDrone.slx └── README.md /DEPRECATED/BrushedFlightController-AltHold-Multiwii/BMX055.h: -------------------------------------------------------------------------------- 1 | #ifndef BMX055_LIB 2 | #define BMX055_LIB 3 | 4 | #include 5 | #include "math.h" 6 | #include "i2c_wrapper.h" 7 | 8 | #define DEBUG 0 9 | 10 | #define CALIBRATE_YAW 1 11 | #define USE_MAG_CALIBRATION 1 12 | #define USE_MAG_RAWDATA 0 13 | #define MAG_TARGET 3.14159// 1.57079632679f = pi/2 14 | 15 | // Accelerometer registers 16 | #define AM_CHIPID_REG 0x00 // Chip ud should be 0xFA 17 | #define AM_BGW_SOFT_RESET 0x14 // Reset register 18 | #define AM_PMU_RANGE 0x0F // G range 19 | #define AM_PMU_BW 0x10 // Bandwidth 20 | #define AM_OFC_CTRL 0x36 // Configuracion de Fast/slow Offset Compensation 21 | #define AM_OFC_SETTING 0x37 // Configuracion de Fast/slow Offset Compensation 22 | #define AM_OFC_OFFSET_X 0x38 // Offset value x, 8 bits 23 | #define AM_OFC_OFFSET_Y 0x39 // Offset value y, 8 bits 24 | #define AM_OFC_OFFSET_Z 0x3A // Offset value z, 8 bits 25 | #define AM_ACCD_X_LSB 0x02 // LSB Data for accelerometer in X 26 | #define AM_ACCD_Y_LSB 0x04 // LSB Data for accelerometer in Y 27 | #define AM_ACCD_Z_LSB 0x06 // LSB Data for accelerometer in Z 28 | 29 | // Gyroscope registers 30 | #define G_CHIPID_REG 0x00 // Should be 0x0F 31 | #define G_BGW_SOFT_RESET 0x14 // Reset register 32 | #define G_RANGE 0x0F // Range settings 33 | #define G_BW 0x10 // Bandwidth settings 34 | #define G_RATE_HBW 0x13 // Select (un)filtered output and shadowing enabling 35 | #define G_A_FOC 0x32 // Fast offset compensation settings 36 | #define G_OFC1 0x36 37 | #define G_OFC2 0x37 38 | #define G_OFC3 0x38 39 | #define G_OFC4 0x39 40 | #define G_TRIM_GP0 0x3A 41 | #define G_RATE_X_LSB 0x02 // LSB Data for gyroscope in X 42 | #define G_RATE_Y_LSB 0x04 // LSB Data for gyroscope in Y 43 | #define G_RATE_Z_LSB 0x06 // LSB Data for gyroscope in Z 44 | 45 | // Magnetometer registers 46 | #define MAG_CHIPID_REG 0x40 // Should be 0x32 47 | #define MAG_SOFT_RESET 0x4B // Reset register 48 | #define MAG_MODR 0x4C 49 | #define MAG_REPXY 0x51 50 | #define MAG_REPZ 0x52 51 | #define MAG_DATAX_LSB 0x42 // LSB Data for magnetometer in X 52 | #define MAG_DATAY_LSB 0x44 // LSB Data for magnetometer in Y 53 | #define MAG_DATAZ_LSB 0x46 // LSB Data for magnetometer in z 54 | #define MAG_RHALL_LSB 0x48 // LSB Data for magnetometer hall resistor 55 | 56 | // Magnetometer extended registers 57 | #define BMM150_DIG_X1 UINT8_C(0x5D) 58 | #define BMM150_DIG_Y1 UINT8_C(0x5E) 59 | #define BMM150_DIG_Z4_LSB UINT8_C(0x62) 60 | #define BMM150_DIG_Z4_MSB UINT8_C(0x63) 61 | #define BMM150_DIG_X2 UINT8_C(0x64) 62 | #define BMM150_DIG_Y2 UINT8_C(0x65) 63 | #define BMM150_DIG_Z2_LSB UINT8_C(0x68) 64 | #define BMM150_DIG_Z2_MSB UINT8_C(0x69) 65 | #define BMM150_DIG_Z1_LSB UINT8_C(0x6A) 66 | #define BMM150_DIG_Z1_MSB UINT8_C(0x6B) 67 | #define BMM150_DIG_XYZ1_LSB UINT8_C(0x6C) 68 | #define BMM150_DIG_XYZ1_MSB UINT8_C(0x6D) 69 | #define BMM150_DIG_Z3_LSB UINT8_C(0x6E) 70 | #define BMM150_DIG_Z3_MSB UINT8_C(0x6F) 71 | #define BMM150_DIG_XY2 UINT8_C(0x70) 72 | #define BMM150_DIG_XY1 UINT8_C(0x71) 73 | 74 | typedef struct { 75 | uint8_t chipid; 76 | uint16_t range; 77 | float res; 78 | float bandwidth; 79 | float sample_time; 80 | double x_offset; 81 | double y_offset; 82 | double z_offset; 83 | double x; 84 | double y; 85 | double z; 86 | }SensorVars; 87 | 88 | typedef struct { 89 | // trim values 90 | int8_t dig_x1; 91 | int8_t dig_y1; 92 | int8_t dig_x2; 93 | int8_t dig_y2; 94 | uint16_t dig_z1; 95 | int16_t dig_z2; 96 | int16_t dig_z3; 97 | int16_t dig_z4; 98 | uint8_t dig_xy1; 99 | int8_t dig_xy2; 100 | uint16_t dig_xyz1; 101 | 102 | // Configuration values 103 | uint8_t chipid; 104 | float res = 1.0/16.0; //uT or 1/1.6 milliGaus per LSB x*0.0625 = y*0.1 105 | uint8_t odr; 106 | String opmode; 107 | int repetitions_xy; 108 | int repetitions_z; 109 | 110 | 111 | // Magnetometer measurements 112 | double x_offset = -3.511194; 113 | double y_offset = -34.861936; 114 | double z_offset = -35.088470; 115 | double soft_iron[9] = {16.050133, 0.041485, -0.194715, 0.041485, 15.947023, 0.036212, -0.194715, 0.036212, 17.987555}; 116 | double yaw_offset = 0; 117 | double x; 118 | double y; 119 | double z; 120 | 121 | }MagnetometerVars; 122 | 123 | class BMX055 124 | { 125 | public: 126 | // Sensor addresses 127 | uint8_t Addr_Accel; 128 | uint8_t Addr_Gyro; 129 | uint8_t Addr_magnet; 130 | uint8_t mag_cal; 131 | 132 | // Constructor/destructor 133 | BMX055(uint8_t AddrAccel, uint8_t AddrGyro, uint8_t Addrmagnet, uint8_t magcal); 134 | ~BMX055(); 135 | 136 | // Accelerometer functions and struct 137 | SensorVars accelerometer; 138 | void acc_init(); 139 | void get_acc_data(); 140 | void get_acc_settings(); 141 | 142 | // Gyroscope functions and struct 143 | SensorVars gyroscope; 144 | int16_t ACC_1G; 145 | void gyr_init(); 146 | void get_gyr_data(); 147 | void get_gyr_settings(); 148 | 149 | // Magnetometer functions and struct 150 | MagnetometerVars magnetometer; 151 | void mag_init(); 152 | void mag_read_trim_registers(); 153 | void get_mag_data(); 154 | void get_mag_settings(); 155 | void mag_yaw_calibration(uint8_t samples); 156 | float mag_compensate_x(int16_t mag_data_x, uint16_t data_rhall); 157 | float mag_compensate_y(int16_t mag_data_y, uint16_t data_rhall); 158 | float mag_compensate_z(int16_t mag_data_z, uint16_t data_rhall); 159 | 160 | private: 161 | uint8_t acc_read(uint8_t reg); 162 | uint8_t acc_write(uint8_t reg, uint8_t data); 163 | uint8_t gyr_read(uint8_t reg); 164 | uint8_t gyr_write(uint8_t reg, uint8_t data); 165 | uint8_t mag_read(uint8_t reg); 166 | uint8_t mag_write(uint8_t reg, uint8_t data); 167 | int16_t twos_comp(uint16_t val, uint8_t bits); 168 | }; 169 | 170 | #endif 171 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-AltHold-Multiwii/Estimates.cpp: -------------------------------------------------------------------------------- 1 | #include "Estimates.h" 2 | 3 | uint16_t missing_samples = 0; 4 | float accSum = 0; 5 | float accTimeSum = 0; 6 | uint16_t accSumCount = 0; 7 | 8 | int32_t baroPressure = 0; 9 | int32_t baroTemperature = 0; 10 | uint32_t baroPressureSum = 0; 11 | uint16_t missing_alt_samples = 0; 12 | int32_t BaroAlt = 0; 13 | 14 | void get_gyr_compensated_data() 15 | { 16 | imu.get_gyr_data(); 17 | } 18 | 19 | void get_acc_compensated_data() 20 | { 21 | static float a[3]; 22 | 23 | imu.get_acc_data(); 24 | 25 | if(calibrate_acc) 26 | { 27 | missing_samples = CAL_SAMPLES; 28 | calibrate_acc = 0; 29 | } 30 | if(missing_samples > 0) 31 | { 32 | missing_samples--; 33 | 34 | if (missing_samples == 511) 35 | { 36 | a[0]=0; 37 | a[1]=0; 38 | a[2]=0; 39 | } 40 | 41 | a[0] += imu.accelerometer.x; 42 | a[1] += imu.accelerometer.y; 43 | a[2] += imu.accelerometer.z; 44 | 45 | acc_calibration[0] = a[0]/512; 46 | acc_calibration[1] = a[1]/512; 47 | acc_calibration[2] = a[2]/512; 48 | 49 | if(missing_samples == 0) 50 | { 51 | acc_calibration[2] -= imu.ACC_1G; 52 | 53 | int address = 0; 54 | EEPROM.put(address, acc_calibration[0]); 55 | address += sizeof(float); 56 | EEPROM.put(address, acc_calibration[1]); 57 | address += sizeof(float); 58 | EEPROM.put(address, acc_calibration[2]); 59 | EEPROM.commit(); 60 | } 61 | } 62 | 63 | imu.accelerometer.x -= acc_calibration[0]; 64 | imu.accelerometer.y -= acc_calibration[1]; 65 | imu.accelerometer.z -= acc_calibration[2]; 66 | 67 | } 68 | 69 | // cfg.baro_tab_size = 21; 70 | //#define BARO_TAB_SIZE_MAX 48 71 | void Baro_Common(void) 72 | { 73 | static int32_t baroHistTab[48]; 74 | static int baroHistIdx; 75 | int indexplus1; 76 | 77 | indexplus1 = (baroHistIdx + 1); 78 | if (indexplus1 == 21) 79 | indexplus1 = 0; 80 | baroHistTab[baroHistIdx] = baroPressure; 81 | baroPressureSum += baroHistTab[baroHistIdx]; 82 | baroPressureSum -= baroHistTab[indexplus1]; 83 | baroHistIdx = indexplus1; 84 | } 85 | 86 | uint8_t get_baro_data() 87 | { 88 | static int state = 0; 89 | static uint32_t previousT; 90 | uint32_t currentT = micros(); 91 | float dt = currentT - previousT; 92 | 93 | if (dt < altimeter.ct*1000.0) 94 | return 0; 95 | previousT = currentT; 96 | 97 | uint32_t readRawTemperature(void); 98 | uint32_t readRawPressure(void); 99 | 100 | if(!state) 101 | { 102 | altimeter.readRawTemperature(); 103 | altimeter.requestPressure(); 104 | Baro_Common(); // LPF using Rolling memory 105 | state = 1; 106 | return 1; 107 | } 108 | else//if(state) 109 | { 110 | altimeter.readRawPressure(); 111 | altimeter.requestTemperature(); 112 | altimeter.calculatePressure(&baroPressure, &baroTemperature); 113 | state = 0; 114 | return 2; 115 | } 116 | } 117 | 118 | float applyDeadband(float value, float deadband) 119 | { 120 | if (abs(value) < deadband) { 121 | value = 0; 122 | } else if (value > 0) { 123 | value -= deadband; 124 | } else if (value < 0) { 125 | value += deadband; 126 | } 127 | return value; 128 | } 129 | 130 | void attitude_estimation(float dt) 131 | { 132 | static float acc_x = 0; 133 | static float acc_y = 0; 134 | static float acc_z = 0; 135 | 136 | static float gyr_roll = 0; 137 | static float gyr_pitch = 0; 138 | 139 | //static uint32_t previousT; 140 | //uint32_t currentT = micros(); 141 | //float dt = currentT - previousT; 142 | //previousT = currentT; 143 | 144 | acc_x = 0.99*acc_x + 0.01*imu.accelerometer.x*imu.accelerometer.res; 145 | acc_y = 0.99*acc_y + 0.01*imu.accelerometer.y*imu.accelerometer.res; 146 | acc_z = 0.99*acc_z + 0.01*imu.accelerometer.z*imu.accelerometer.res; 147 | 148 | float accMag = 100.0*(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z)/(9.81*9.81); 149 | 150 | float acc_roll = atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z)); 151 | float acc_pitch = atan2(-1.0*acc_x, sqrt(acc_y*acc_y + acc_z*acc_z)); 152 | 153 | gyr_roll = gyr_roll*0.95 + 0.05*imu.gyroscope.x*imu.gyroscope.res; 154 | gyr_pitch = gyr_pitch*0.95 + 0.05*imu.gyroscope.y*imu.gyroscope.res; 155 | 156 | //if (72 < accMag && accMag < 133) { 157 | if (80 < accMag && accMag < 120) { 158 | roll = 0.9*(roll + gyr_roll*dt*PI/(1000000.0*180.0)) + 0.1*acc_roll; 159 | pitch = 0.9*(pitch + gyr_pitch*dt*PI/(1000000.0*180.0)) + 0.1*acc_pitch; 160 | } 161 | else 162 | { 163 | roll = roll + gyr_roll*dt*PI/(1000000.0*180.0); 164 | pitch = pitch + gyr_pitch*dt*PI/(1000000.0*180.0); 165 | } 166 | } 167 | 168 | void acceleration_estimation(float dt) 169 | { 170 | float world_acc_z = imu.accelerometer.z*cos(pitch)*cos(roll) - imu.accelerometer.x*sin(pitch) + imu.accelerometer.y*cos(pitch)*sin(roll); 171 | world_acc_z -= imu.ACC_1G; 172 | 173 | acczSmooth = 0.95*acczSmooth + 0.05*world_acc_z; 174 | 175 | accSum += applyDeadband(acczSmooth, imu.ACC_1G/32); 176 | accTimeSum += dt; 177 | accSumCount++; 178 | } 179 | 180 | #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) 181 | 182 | uint8_t altitude_estimation() 183 | { 184 | static int32_t baroGroundPressure = 0; 185 | static int32_t baroGroundAltitude = 0; 186 | 187 | int32_t BaroAlt_tmp; 188 | static int32_t lastBaroAlt; 189 | int32_t baroVel; 190 | static float vel = 0.0f; 191 | static float lpf_vel = 0.0f; 192 | static float accAlt = 0.0f; 193 | static float accZ_tmp = 0; 194 | static float accZ_old = 0.0f; 195 | 196 | static uint32_t previousT; 197 | uint32_t currentT = micros(); 198 | float dt = currentT - previousT; 199 | 200 | // Update with a constant period 201 | if (dt < UPDATE_INTERVAL) 202 | return 0; 203 | previousT = currentT; 204 | 205 | if(calibrate_alt) 206 | { 207 | missing_alt_samples = BARO_CAL_SAMPLES; 208 | calibrate_alt = 0; 209 | } 210 | 211 | if(missing_alt_samples > 0) 212 | { 213 | 214 | baroGroundPressure = 0.9*baroGroundPressure + 0.1*(baroPressureSum / (21 - 1)); 215 | baroGroundAltitude = (1.0f - powf(baroGroundPressure / 101325.0f, 0.190295f)) * 4433000.0f; 216 | 217 | vel = 0; 218 | accAlt = 0; 219 | EstAlt = 0; 220 | missing_alt_samples--; 221 | } 222 | 223 | BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (21 - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm 224 | BaroAlt_tmp -= baroGroundAltitude; 225 | //BaroAlt = lrintf((float)BaroAlt * 0.6 + (float)BaroAlt_tmp * 0.4); // additional LPF to reduce baro noise 226 | BaroAlt = lrintf((float)BaroAlt * 0.9 + (float)BaroAlt_tmp * 0.1); // additional LPF to reduce baro noise 227 | 228 | float acc_dt = accTimeSum * 1e-6f; 229 | accZ_tmp = 0.7*accZ_tmp + 0.3*(float)accSum / (float)accSumCount; 230 | 231 | float vel_acc = accZ_tmp * imu.accelerometer.res*100.0 * acc_dt; // vel diff in cm/s 232 | 233 | //accAlt += (vel_acc * 0.5f) * acc_dt + vel * acc_dt; // integrate velocity to get distance (x= a/2 * t^2) 234 | //accAlt = accAlt * 0.965 + (float)BaroAlt * 0.035; // complementary filter for altitude estimation (baro & acc) 235 | 236 | accAlt = (vel_acc * 0.5f) * acc_dt + vel * acc_dt; // integrate velocity to get distance (x= a/2 * t^2) 237 | EstAlt = (EstAlt + accAlt) * 0.9 + (float)BaroAlt * 0.1; // complementary filter for altitude estimation (baro & acc) 238 | 239 | vel += vel_acc; 240 | 241 | //accSum_reset(); 242 | accSum = 0; 243 | accTimeSum = 0; 244 | accSumCount = 0; 245 | 246 | baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dt; 247 | lastBaroAlt = BaroAlt; 248 | 249 | baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s 250 | baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero 251 | lpf_vel = 0.9*lpf_vel + 0.1*baroVel; 252 | 253 | vel = vel * 0.9 + lpf_vel * 0.1; 254 | //vel = vel * 0.985 + baroVel * 0.015; 255 | //lpf_vel = 0.7*lpf_vel + 0.3*vel; 256 | int32_t vel_tmp = lrintf(vel); 257 | //Serial.print(BaroAlt); 258 | //Serial.print(" "); 259 | //Serial.println(EstAlt); 260 | 261 | //Serial.print(baroVel); 262 | //Serial.print(" "); 263 | //Serial.println(lpf_vel); 264 | 265 | //Serial.print(accZ_tmp); 266 | //Serial.print(" "); 267 | //Serial.println(vel); 268 | 269 | int32_t error; 270 | int32_t setVel; 271 | uint8_t alt_p = 25;//50; 272 | uint8_t vel_kp = 80;//50;//120; 273 | uint8_t vel_ki = 0;//45; 274 | uint8_t vel_kd = 200;//1; 275 | 276 | if(roll*180/PI < 60 && pitch*180/PI < 60) 277 | { 278 | if(true) 279 | { 280 | error = constrain(AltitudeSetpoint-EstAlt, -500, 500); 281 | error = applyDeadband(error, 10); 282 | setVel = constrain((alt_p * error / 128), -300, +300); // limit velocity to +/- 3 m/s 283 | //Serial.println(error); 284 | } 285 | else 286 | { 287 | setVel = 0; 288 | } 289 | 290 | // Proportianl term 291 | error = setVel - vel_tmp; 292 | BaroPID = constrain(vel_kp*error/32, -300, 300); 293 | 294 | // Integral term 295 | errorVelocityI += (vel_ki * error); 296 | errorVelocityI = constrain(errorVelocityI, -(8196 * 200), (8196 * 200)); 297 | BaroPID += errorVelocityI / 8196; 298 | 299 | // Derivative term 300 | BaroPID -= constrain(vel_kd * (accZ_tmp + accZ_old) / 512, -150, 150); 301 | 302 | //Serial.print(BaroAlt); 303 | //Serial.print(" "); 304 | //Serial.println(EstAlt); 305 | //Serial.println(vel_tmp); 306 | } 307 | else 308 | { 309 | BaroPID = 0; 310 | } 311 | 312 | accZ_old = accZ_tmp; 313 | return 1; 314 | } 315 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-AltHold-Multiwii/Estimates.h: -------------------------------------------------------------------------------- 1 | #ifndef ESTIMATES_LIB 2 | #define ESTIMATES_LIB 3 | 4 | #include 5 | #include "BMX055.h" 6 | #include "MS5611.h" 7 | #include 8 | 9 | #define CAL_SAMPLES 512 10 | #define BARO_CAL_SAMPLES 250 11 | 12 | extern BMX055 imu; 13 | extern uint8_t calibrate_acc; 14 | extern float acc_calibration[3]; 15 | extern float roll; 16 | extern float pitch; 17 | extern float acczSmooth; 18 | 19 | extern MS5611 altimeter; 20 | extern uint8_t calibrate_alt; 21 | extern int32_t BaroPID; 22 | extern int32_t errorVelocityI; 23 | extern int32_t EstAlt; 24 | extern int32_t AltitudeSetpoint; 25 | 26 | void get_gyr_compensated_data(); 27 | void get_acc_compensated_data(); 28 | uint8_t get_baro_data(); 29 | float applyDeadband(float value, float deadband); 30 | void attitude_estimation(float dt); 31 | void acceleration_estimation(float dt); 32 | uint8_t altitude_estimation(); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-AltHold-Multiwii/MS5611.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611.cpp - Class file for the MS5611 Barometric Pressure & Temperature Sensor Arduino Library. 3 | 4 | Version: 1.0.0 5 | (c) 2014 Korneliusz Jarzebski 6 | www.jarzebski.pl 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the version 3 GNU General Public License as 10 | published by the Free Software Foundation. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | #include "Arduino.h" 22 | 23 | #include 24 | #include 25 | 26 | #include "MS5611.h" 27 | 28 | bool MS5611::begin(ms5611_osr_t osr) 29 | { 30 | 31 | reset(); 32 | 33 | setOversampling(osr); 34 | 35 | delay(100); 36 | 37 | readPROM(); 38 | 39 | return true; 40 | } 41 | 42 | // Set oversampling value 43 | void MS5611::setOversampling(ms5611_osr_t osr) 44 | { 45 | switch (osr) 46 | { 47 | case MS5611_ULTRA_LOW_POWER: 48 | ct = 1; 49 | break; 50 | case MS5611_LOW_POWER: 51 | ct = 2; 52 | break; 53 | case MS5611_STANDARD: 54 | ct = 3; 55 | break; 56 | case MS5611_HIGH_RES: 57 | ct = 5; 58 | break; 59 | case MS5611_ULTRA_HIGH_RES: 60 | ct = 10; 61 | break; 62 | } 63 | 64 | uosr = osr; 65 | } 66 | 67 | // Get oversampling value 68 | uint8_t MS5611::getOversampling(void) 69 | { 70 | //return (ms5611_osr_t)uosr; 71 | return ct; 72 | } 73 | 74 | void MS5611::reset(void) 75 | { 76 | Wire.beginTransmission(MS5611_ADDRESS); 77 | Wire.write(MS5611_CMD_RESET); 78 | Wire.endTransmission(); 79 | } 80 | 81 | void MS5611::readPROM(void) 82 | { 83 | for (uint8_t offset = 0; offset < 6; offset++) 84 | { 85 | fc[offset] = readRegister16(MS5611_CMD_READ_PROM + (offset * 2)); 86 | } 87 | } 88 | 89 | void MS5611::requestTemperature(void) 90 | { 91 | Wire.beginTransmission(MS5611_ADDRESS); 92 | Wire.write(MS5611_CMD_CONV_D2 + uosr); 93 | Wire.endTransmission(); 94 | } 95 | 96 | void MS5611::requestPressure(void) 97 | { 98 | Wire.beginTransmission(MS5611_ADDRESS); 99 | Wire.write(MS5611_CMD_CONV_D1 + uosr); 100 | Wire.endTransmission(); 101 | } 102 | 103 | uint32_t MS5611::readRawTemperature(void) 104 | { 105 | raw_temperature = readRegister24(MS5611_CMD_ADC_READ); 106 | return raw_temperature; 107 | //return readRegister24(MS5611_CMD_ADC_READ); 108 | } 109 | 110 | uint32_t MS5611::readRawPressure(void) 111 | { 112 | raw_pressure = readRegister24(MS5611_CMD_ADC_READ); 113 | return raw_pressure; 114 | //return readRegister24(MS5611_CMD_ADC_READ); 115 | } 116 | 117 | int32_t MS5611::readPressure(uint32_t rawTemperature, bool compensation) 118 | { 119 | uint32_t D1 = readRawPressure(); 120 | 121 | uint32_t D2 = rawTemperature; 122 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 123 | 124 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 125 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 126 | 127 | if (compensation) 128 | { 129 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 130 | 131 | OFF2 = 0; 132 | SENS2 = 0; 133 | 134 | if (TEMP < 2000) 135 | { 136 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 137 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 138 | } 139 | 140 | if (TEMP < -1500) 141 | { 142 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 143 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 144 | } 145 | 146 | OFF = OFF - OFF2; 147 | SENS = SENS - SENS2; 148 | } 149 | 150 | uint32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 151 | 152 | return P; 153 | } 154 | 155 | int32_t MS5611::readPressure(bool compensation) 156 | { 157 | requestPressure(); 158 | delay(ct); 159 | uint32_t D1 = readRawPressure(); 160 | 161 | requestTemperature(); 162 | delay(ct); 163 | uint32_t D2 = readRawTemperature(); 164 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 165 | 166 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 167 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 168 | 169 | if (compensation) 170 | { 171 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 172 | 173 | OFF2 = 0; 174 | SENS2 = 0; 175 | 176 | if (TEMP < 2000) 177 | { 178 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 179 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 180 | } 181 | 182 | if (TEMP < -1500) 183 | { 184 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 185 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 186 | } 187 | 188 | OFF = OFF - OFF2; 189 | SENS = SENS - SENS2; 190 | } 191 | 192 | uint32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 193 | 194 | return P; 195 | } 196 | 197 | void MS5611::calculatePressure(int32_t *pressure, int32_t *temperature) 198 | { 199 | uint32_t D1 = raw_pressure; 200 | 201 | uint32_t D2 = raw_temperature; 202 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 203 | 204 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 205 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 206 | 207 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 208 | 209 | OFF2 = 0; 210 | SENS2 = 0; 211 | 212 | if (TEMP < 2000) 213 | { 214 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 215 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 216 | } 217 | 218 | if (TEMP < -1500) 219 | { 220 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 221 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 222 | } 223 | 224 | OFF = OFF - OFF2; 225 | SENS = SENS - SENS2; 226 | 227 | int32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 228 | 229 | if (pressure) 230 | *pressure = P; 231 | if (temperature) 232 | *temperature = TEMP; 233 | } 234 | 235 | double MS5611::readTemperature(bool compensation) 236 | { 237 | uint32_t D2 = readRawTemperature(); 238 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 239 | 240 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 241 | 242 | TEMP2 = 0; 243 | 244 | if (compensation) 245 | { 246 | if (TEMP < 2000) 247 | { 248 | TEMP2 = (dT * dT) / (2 << 30); 249 | } 250 | } 251 | 252 | TEMP = TEMP - TEMP2; 253 | 254 | return ((double)TEMP/100); 255 | } 256 | 257 | // Calculate altitude from Pressure & Sea level pressure 258 | double MS5611::getAltitude(double pressure, double seaLevelPressure) 259 | { 260 | return (44330.0f * (1.0f - pow((double)pressure / (double)seaLevelPressure, 0.1902949f))); 261 | } 262 | 263 | // Calculate sea level from Pressure given on specific altitude 264 | double MS5611::getSeaLevel(double pressure, double altitude) 265 | { 266 | return ((double)pressure / pow(1.0f - ((double)altitude / 44330.0f), 5.255f)); 267 | } 268 | 269 | // Read 16-bit from register (oops MSB, LSB) 270 | uint16_t MS5611::readRegister16(uint8_t reg) 271 | { 272 | uint16_t value; 273 | Wire.beginTransmission(MS5611_ADDRESS); 274 | Wire.write(reg); 275 | Wire.endTransmission(); 276 | 277 | Wire.beginTransmission(MS5611_ADDRESS); 278 | Wire.requestFrom(MS5611_ADDRESS, 2); 279 | while(!Wire.available()) {}; 280 | uint8_t vha = Wire.read(); 281 | uint8_t vla = Wire.read(); 282 | Wire.endTransmission(); 283 | 284 | value = vha << 8 | vla; 285 | 286 | return value; 287 | } 288 | 289 | // Read 24-bit from register (oops XSB, MSB, LSB) 290 | uint32_t MS5611::readRegister24(uint8_t reg) 291 | { 292 | uint32_t value; 293 | Wire.beginTransmission(MS5611_ADDRESS); 294 | Wire.write(reg); 295 | Wire.endTransmission(); 296 | 297 | Wire.beginTransmission(MS5611_ADDRESS); 298 | Wire.requestFrom(MS5611_ADDRESS, 3); 299 | while(!Wire.available()) {}; 300 | uint8_t vxa = Wire.read(); 301 | uint8_t vha = Wire.read(); 302 | uint8_t vla = Wire.read(); 303 | Wire.endTransmission(); 304 | 305 | value = ((int32_t)vxa << 16) | ((int32_t)vha << 8) | vla; 306 | 307 | return value; 308 | } 309 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-AltHold-Multiwii/MS5611.h: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611.h - Header file for the MS5611 Barometric Pressure & Temperature Sensor Arduino Library. 3 | 4 | Version: 1.0.0 5 | (c) 2014 Korneliusz Jarzebski 6 | www.jarzebski.pl 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the version 3 GNU General Public License as 10 | published by the Free Software Foundation. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | #ifndef MS5611_h 22 | #define MS5611_h 23 | 24 | #include "Arduino.h" 25 | 26 | #define MS5611_ADDRESS (0x77) 27 | //#define MS5611_ADDRESS (0x76) 28 | 29 | #define MS5611_CMD_ADC_READ (0x00) 30 | #define MS5611_CMD_RESET (0x1E) 31 | #define MS5611_CMD_CONV_D1 (0x40) 32 | #define MS5611_CMD_CONV_D2 (0x50) 33 | #define MS5611_CMD_READ_PROM (0xA2) 34 | 35 | typedef enum 36 | { 37 | MS5611_ULTRA_HIGH_RES = 0x08, 38 | MS5611_HIGH_RES = 0x06, 39 | MS5611_STANDARD = 0x04, 40 | MS5611_LOW_POWER = 0x02, 41 | MS5611_ULTRA_LOW_POWER = 0x00 42 | } ms5611_osr_t; 43 | 44 | class MS5611 45 | { 46 | public: 47 | 48 | uint8_t ct; 49 | 50 | bool begin(ms5611_osr_t osr = MS5611_HIGH_RES); 51 | void requestTemperature(void); 52 | void requestPressure(void); 53 | uint32_t readRawTemperature(void); 54 | uint32_t readRawPressure(void); 55 | 56 | double readTemperature(bool compensation = false); 57 | int32_t readPressure(bool compensation = false); 58 | int32_t readPressure(uint32_t rawTemperature, bool compensation = false); 59 | void calculatePressure(int32_t *pressure, int32_t *temperature); 60 | double getAltitude(double pressure, double seaLevelPressure = 101325); 61 | double getSeaLevel(double pressure, double altitude); 62 | void setOversampling(ms5611_osr_t osr); 63 | uint8_t getOversampling(void); 64 | 65 | private: 66 | 67 | uint16_t fc[6]; 68 | uint8_t uosr; 69 | int32_t TEMP2; 70 | int64_t OFF2, SENS2; 71 | 72 | uint32_t raw_temperature; 73 | uint32_t raw_pressure; 74 | 75 | void reset(void); 76 | void readPROM(void); 77 | 78 | uint16_t readRegister16(uint8_t reg); 79 | uint32_t readRegister24(uint8_t reg); 80 | }; 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-AltHold-Multiwii/i2c_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_WRAPPER 2 | #define I2C_WRAPPER 3 | 4 | #include 5 | 6 | #define SUCCESS 0 7 | #define ADDR_ERR 1 8 | #define DATA_ERR 2 9 | 10 | static uint8_t i2cset(uint8_t address, uint8_t *data, uint8_t len, bool stp) 11 | { 12 | //address = (address<<1); 13 | Wire.beginTransmission(address); 14 | for(int i = 0; i < len; i++) { Wire.write(data[i]); } 15 | // This needs to return 0 16 | return Wire.endTransmission(stp); 17 | } 18 | 19 | static void i2cget(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) 20 | { 21 | uint8_t tx_data[1] = {reg}; 22 | i2cset(address, tx_data, 1, false); 23 | 24 | Wire.requestFrom(address, len); 25 | uint8_t i = 0; 26 | while(Wire.available()){ 27 | if(len == i) 28 | return; 29 | data[i++] = Wire.read(); 30 | } 31 | } 32 | #endif 33 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/BMX055.h: -------------------------------------------------------------------------------- 1 | #ifndef BMX055_LIB 2 | #define BMX055_LIB 3 | 4 | #include 5 | #include "math.h" 6 | #include "i2c_wrapper.h" 7 | 8 | #define DEBUG 1 9 | 10 | #define CALIBRATE_YAW 1 11 | #define USE_MAG_CALIBRATION 1 12 | #define USE_MAG_RAWDATA 0 13 | #define MAG_TARGET 3.14159// 1.57079632679f = pi/2 14 | 15 | // Accelerometer registers 16 | #define AM_CHIPID_REG 0x00 // Chip ud should be 0xFA 17 | #define AM_BGW_SOFT_RESET 0x14 // Reset register 18 | #define AM_PMU_RANGE 0x0F // G range 19 | #define AM_PMU_BW 0x10 // Bandwidth 20 | #define AM_OFC_CTRL 0x36 // Configuracion de Fast/slow Offset Compensation 21 | #define AM_OFC_SETTING 0x37 // Configuracion de Fast/slow Offset Compensation 22 | #define AM_OFC_OFFSET_X 0x38 // Offset value x, 8 bits 23 | #define AM_OFC_OFFSET_Y 0x39 // Offset value y, 8 bits 24 | #define AM_OFC_OFFSET_Z 0x3A // Offset value z, 8 bits 25 | #define AM_ACCD_X_LSB 0x02 // LSB Data for accelerometer in X 26 | #define AM_ACCD_Y_LSB 0x04 // LSB Data for accelerometer in Y 27 | #define AM_ACCD_Z_LSB 0x06 // LSB Data for accelerometer in Z 28 | 29 | // Gyroscope registers 30 | #define G_CHIPID_REG 0x00 // Should be 0x0F 31 | #define G_BGW_SOFT_RESET 0x14 // Reset register 32 | #define G_RANGE 0x0F // Range settings 33 | #define G_BW 0x10 // Bandwidth settings 34 | #define G_RATE_HBW 0x13 // Select (un)filtered output and shadowing enabling 35 | #define G_A_FOC 0x32 // Fast offset compensation settings 36 | #define G_OFC1 0x36 37 | #define G_OFC2 0x37 38 | #define G_OFC3 0x38 39 | #define G_OFC4 0x39 40 | #define G_TRIM_GP0 0x3A 41 | #define G_RATE_X_LSB 0x02 // LSB Data for gyroscope in X 42 | #define G_RATE_Y_LSB 0x04 // LSB Data for gyroscope in Y 43 | #define G_RATE_Z_LSB 0x06 // LSB Data for gyroscope in Z 44 | 45 | // Magnetometer registers 46 | #define MAG_CHIPID_REG 0x40 // Should be 0x32 47 | #define MAG_SOFT_RESET 0x4B // Reset register 48 | #define MAG_MODR 0x4C 49 | #define MAG_REPXY 0x51 50 | #define MAG_REPZ 0x52 51 | #define MAG_DATAX_LSB 0x42 // LSB Data for magnetometer in X 52 | #define MAG_DATAY_LSB 0x44 // LSB Data for magnetometer in Y 53 | #define MAG_DATAZ_LSB 0x46 // LSB Data for magnetometer in z 54 | #define MAG_RHALL_LSB 0x48 // LSB Data for magnetometer hall resistor 55 | 56 | // Magnetometer extended registers 57 | #define BMM150_DIG_X1 UINT8_C(0x5D) 58 | #define BMM150_DIG_Y1 UINT8_C(0x5E) 59 | #define BMM150_DIG_Z4_LSB UINT8_C(0x62) 60 | #define BMM150_DIG_Z4_MSB UINT8_C(0x63) 61 | #define BMM150_DIG_X2 UINT8_C(0x64) 62 | #define BMM150_DIG_Y2 UINT8_C(0x65) 63 | #define BMM150_DIG_Z2_LSB UINT8_C(0x68) 64 | #define BMM150_DIG_Z2_MSB UINT8_C(0x69) 65 | #define BMM150_DIG_Z1_LSB UINT8_C(0x6A) 66 | #define BMM150_DIG_Z1_MSB UINT8_C(0x6B) 67 | #define BMM150_DIG_XYZ1_LSB UINT8_C(0x6C) 68 | #define BMM150_DIG_XYZ1_MSB UINT8_C(0x6D) 69 | #define BMM150_DIG_Z3_LSB UINT8_C(0x6E) 70 | #define BMM150_DIG_Z3_MSB UINT8_C(0x6F) 71 | #define BMM150_DIG_XY2 UINT8_C(0x70) 72 | #define BMM150_DIG_XY1 UINT8_C(0x71) 73 | 74 | typedef struct { 75 | uint8_t chipid; 76 | uint16_t range; 77 | float res; 78 | float bandwidth; 79 | float sample_time; 80 | double x_offset; 81 | double y_offset; 82 | double z_offset; 83 | double x; 84 | double y; 85 | double z; 86 | }SensorVars; 87 | 88 | typedef struct { 89 | // trim values 90 | int8_t dig_x1; 91 | int8_t dig_y1; 92 | int8_t dig_x2; 93 | int8_t dig_y2; 94 | uint16_t dig_z1; 95 | int16_t dig_z2; 96 | int16_t dig_z3; 97 | int16_t dig_z4; 98 | uint8_t dig_xy1; 99 | int8_t dig_xy2; 100 | uint16_t dig_xyz1; 101 | 102 | // Configuration values 103 | uint8_t chipid; 104 | float res = 1.0/16.0; //uT or 1/1.6 milliGaus per LSB x*0.0625 = y*0.1 105 | uint8_t odr; 106 | String opmode; 107 | int repetitions_xy; 108 | int repetitions_z; 109 | 110 | 111 | // Magnetometer measurements 112 | double x_offset = -3.511194; 113 | double y_offset = -34.861936; 114 | double z_offset = -35.088470; 115 | double soft_iron[9] = {16.050133, 0.041485, -0.194715, 0.041485, 15.947023, 0.036212, -0.194715, 0.036212, 17.987555}; 116 | double yaw_offset = 0; 117 | double x; 118 | double y; 119 | double z; 120 | 121 | }MagnetometerVars; 122 | 123 | class BMX055 124 | { 125 | public: 126 | // Sensor addresses 127 | uint8_t Addr_Accel; 128 | uint8_t Addr_Gyro; 129 | uint8_t Addr_magnet; 130 | uint8_t mag_cal; 131 | 132 | // Constructor/destructor 133 | BMX055(uint8_t AddrAccel, uint8_t AddrGyro, uint8_t Addrmagnet, uint8_t magcal); 134 | ~BMX055(); 135 | 136 | // Accelerometer functions and struct 137 | SensorVars accelerometer; 138 | void acc_init(); 139 | void get_acc_data(); 140 | void get_acc_settings(); 141 | 142 | // Gyroscope functions and struct 143 | SensorVars gyroscope; 144 | void gyr_init(); 145 | void get_gyr_data(); 146 | void get_gyr_settings(); 147 | 148 | // Magnetometer functions and struct 149 | MagnetometerVars magnetometer; 150 | void mag_init(); 151 | void mag_read_trim_registers(); 152 | void get_mag_data(); 153 | void get_mag_settings(); 154 | void mag_yaw_calibration(uint8_t samples); 155 | float mag_compensate_x(int16_t mag_data_x, uint16_t data_rhall); 156 | float mag_compensate_y(int16_t mag_data_y, uint16_t data_rhall); 157 | float mag_compensate_z(int16_t mag_data_z, uint16_t data_rhall); 158 | 159 | private: 160 | uint8_t acc_read(uint8_t reg); 161 | uint8_t acc_write(uint8_t reg, uint8_t data); 162 | uint8_t gyr_read(uint8_t reg); 163 | uint8_t gyr_write(uint8_t reg, uint8_t data); 164 | uint8_t mag_read(uint8_t reg); 165 | uint8_t mag_write(uint8_t reg, uint8_t data); 166 | int16_t twos_comp(uint16_t val, uint8_t bits); 167 | }; 168 | 169 | #endif 170 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/CC1125.h: -------------------------------------------------------------------------------- 1 | #ifndef CC1125_H 2 | #define CC1125_H 3 | // based on https://github.com/Evancw/CC112x/blob/master/examples/Rx_demo.ino 4 | #include 5 | #include 6 | 7 | 8 | //**************************** pins ******************************************// 9 | #define SS_PIN SS//2//SS 10 | #define MISO_PIN MISO//19//MISO 11 | #define MISO_PIN MOSI//23//MOSI 12 | 13 | /*---------------------------[CC11XX - R/W and burst bits]---------------------------*/ 14 | #define WRITE_SINGLE_BYTE 0x00 15 | #define WRITE_BURST 0x40 16 | #define READ_SINGLE_BYTE 0x80 17 | #define READ_BURST 0xC0 18 | 19 | /*----------------------[CC11XX - config register]----------------------------*/ 20 | #define IOCFG3 0x00 // GDO3 output pin config 21 | #define IOCFG2 0x01 // GDO2 output pin config 22 | #define IOCFG1 0x02 // GDO1 output pin config 23 | #define IOCFG0 0x03 // GDO0 output pin config 24 | #define SYNC3 0x04 // Sync word configuration 25 | #define SYNC2 0x05 26 | #define SYNC1 0x06 27 | #define SYNC0 0x07 28 | #define SYNC_CFG1 0x08 // Sync word detection 29 | #define SYNC_CFG0 0x09 30 | #define DEVIATION_M 0x0A // Frequency Deviation 31 | #define MODCFG_DEV_E 0x0B // Modulation format and frequency deviation 32 | #define DCFILT_CFG 0x0C // Digital DC Removal 33 | #define PREAMBLE_CFG1 0x0D // Preamble length config 34 | #define PREAMBLE_CFG0 0x0E 35 | #define FREQ_IF_CFG 0x0F // RX Mixer frequency 36 | #define IQIC 0x10 // Digital image channel compensation 37 | #define CHAN_BW 0x11 // Frequency control word, low byte 38 | #define MDMCFG1 0x12 // Modem configuration 39 | #define MDMCFG0 0x13 // Modem configuration 40 | #define SYMBOL_RATE2 0x14 // Modem configuration 41 | #define SYMBOL_RATE1 0x15 // Modem configuration 42 | #define SYMBOL_RATE0 0x16 // Modem configuration 43 | #define AGC_REF 0x17 // Modem deviation setting 44 | #define AGC_CS_THR 0x18 // Main Radio Cntrl State Machine config 45 | #define AGC_GAIN_ADJUST 0x19 // Main Radio Cntrl State Machine config 46 | #define AGC_CFG3 0x1A // Main Radio Cntrl State Machine config 47 | #define AGC_CFG2 0x1B // Frequency Offset Compensation config 48 | #define AGC_CFG1 0x1C // Bit Synchronization configuration 49 | #define AGC_CFG0 0x1D // AGC control 50 | #define FIFO_CFG 0x1E // AGC control 51 | #define DEV_ADDR 0x1F // AGC control 52 | #define SETTLING_CFG 0x20 // High byte Event 0 timeout 53 | #define FS_CFG 0x21 // Low byte Event 0 timeout 54 | #define WOR_CFG1 0x22 // Wake On Radio control 55 | #define WOR_CFG0 0x23 // Front end RX configuration 56 | #define WOR_EVENT0_MSB 0x24 // Front end TX configuration 57 | #define WOR_EVENT0_LSB 0x25 // Frequency synthesizer calibration 58 | #define PKT_CFG2 0x26 // Frequency synthesizer calibration 59 | #define PKT_CFG1 0x27 // Frequency synthesizer calibration 60 | #define PKT_CFG0 0x28 // Frequency synthesizer calibration 61 | #define RFEND_CFG1 0x29 // RC oscillator configuration 62 | #define RFEND_CFG0 0x2A // RC oscillator configuration 63 | #define PA_CFG2 0x2B // Frequency synthesizer cal control 64 | #define PA_CFG1 0x2C // Production test 65 | #define PA_CFG0 0x2D // AGC test 66 | #define PKT_LEN 0x2E // Various test settings 67 | 68 | /*----------------------[CC11XX - misc]---------------------------------------*/ 69 | #define EXTD_REGISTER 0x2F //Extended register space 70 | 71 | /*------------------------[CC11XX-command strobes]----------------------------*/ 72 | #define SRES 0x30 // Reset chip 73 | #define SFSTXON 0x31 // Enable/calibrate freq synthesizer 74 | #define SXOFF 0x32 // Turn off crystal oscillator. 75 | #define SCAL 0x33 // Calibrate freq synthesizer & disable 76 | #define SRX 0x34 // Enable RX. 77 | #define STX 0x35 // Enable TX. 78 | #define SIDLE 0x36 // Exit RX / TX 79 | #define SAFC 0x37 // AFC adjustment of freq synthesizer 80 | #define SWOR 0x38 // Start automatic RX polling sequence 81 | #define SPWD 0x39 // Enter pwr down mode when CSn goes hi 82 | #define SFRX 0x3A // Flush the RX FIFO buffer. 83 | #define SFTX 0x3B // Flush the TX FIFO buffer. 84 | #define SWORRST 0x3C // Reset real time clock. 85 | #define SNOP 0x3D // No operation. 86 | 87 | /*----------------------[CC11XX - extended registers]----------------------------*/ 88 | #define IF_MIX_CFG 0x00 89 | #define TOC_CFG 0x02 90 | #define FREQ2 0x0C 91 | #define FREQ1 0x0D 92 | #define FREQ0 0x0E 93 | #define IF_ADC0 0x11 94 | #define FS_DIG1 0x12 95 | #define FS_DIG0 0x13 96 | #define FS_CAL2 0x15 // used in manual calibration 97 | #define FS_CAL0 0x17 98 | #define FS_CHP 0x18 // used in manual calibration 99 | #define FS_DIVTWO 0x19 100 | #define FS_DSM0 0x1B 101 | #define FS_DVC0 0x1D 102 | #define FS_PFD 0x1F 103 | #define FS_PRE 0x20 104 | #define FS_REG_DIV_CML 0x21 105 | #define FS_SPARE 0x22 106 | #define FS_VCO4 0x23 // used in manual calibration 107 | #define FS_VCO2 0x25 // used in manual calibration 108 | #define XOSC5 0x32 109 | #define XOSC3 0x34 110 | #define XOSC1 0x36 111 | #define MARCSTATE 0x73 // Control state machine state 112 | #define PARTNUM 0x8F // Part number 113 | #define VERSION 0x90 // Current version number 114 | #define NUM_RXBYTES 0xD7 115 | #define FIFO_NUM_RX 0xD9 // Number of available bytes in RXFIFO 116 | 117 | /*------------------------[CC11XX - FIFO commands]----------------------------*/ 118 | #define STANDARD_FIFO 0x3F //write single only 119 | 120 | class CC1125 121 | { 122 | private: 123 | //void commandStrobe(uint8_t instr) {STATUS = spiRead(instr);}; 124 | 125 | uint8_t STATUS; 126 | //SPISettings s = SPISettings(4000000, MSBFIRST, SPI_MODE0); 127 | SPISettings s = SPISettings(8000000, MSBFIRST, SPI_MODE0); 128 | 129 | public: 130 | void commandStrobe(uint8_t instr); 131 | //uninitalised pointers to SPI objects 132 | SPIClass * vspi = NULL; 133 | bool begin(void); 134 | void set_idle(void); 135 | void receive(void); 136 | void transmit(void); 137 | void tx_payload(uint8_t *txbuffer, uint8_t len); 138 | void sendPacket(uint8_t *txbuffer, uint8_t len); 139 | bool get_packet(uint8_t rxbuffer[], uint8_t &pktlen); 140 | void manualCalibration(void); 141 | void spiWrite(uint8_t reg, uint8_t data, uint8_t prefix=0); 142 | uint8_t spiRead(uint8_t reg, uint8_t prefix=0); 143 | }; 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/RollingMemory.h: -------------------------------------------------------------------------------- 1 | #ifndef Memory_h 2 | #define Memory_h 3 | 4 | // Rolling average strcture 5 | typedef struct { 6 | double memory[40] = {0}; 7 | double sum = 0; 8 | uint8_t index = 0; 9 | double samples = 20.0; 10 | uint8_t averaged_samples = 0; 11 | }RollingMemory; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/SensorFusion.cpp: -------------------------------------------------------------------------------- 1 | #include "SensorFusion.h" 2 | 3 | void SensorFusion::init(float initial_roll, float initial_pitch, float initial_yaw) 4 | { 5 | //roll_offset = initial_roll; 6 | //pitch_offset = initial_pitch; 7 | roll = initial_roll; 8 | pitch = initial_pitch; 9 | 10 | #if SHIFTED_YAW 11 | yaw = initial_yaw; 12 | shifted_yaw = 0; 13 | #else 14 | yaw = initial_yaw; 15 | shifted_yaw = initial_yaw; 16 | #endif 17 | prev_time = millis(); 18 | } 19 | 20 | void SensorFusion::set_acc_offsets(float rollOffset, float pitchOffset) 21 | { 22 | roll_offset = rollOffset; 23 | pitch_offset = pitchOffset; 24 | } 25 | 26 | /*SensorFusion::~SensorFusion() 27 | {}*/ 28 | 29 | void SensorFusion::set_yaw_offset(float offset) 30 | { 31 | yaw_offset = offset; 32 | } 33 | void SensorFusion::fuse_sensors(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 34 | { 35 | // Get time delta between updates 36 | unsigned long current_time = millis(); 37 | float t_delta = ((float)current_time - (float)prev_time)/1000; 38 | prev_time = current_time; 39 | 40 | ax_av.sum -= ax_av.memory[ax_av.index]; 41 | ax_av.memory[ax_av.index] = ax; 42 | ax_av.sum += ax_av.memory[ax_av.index]; 43 | ax_av.index++; 44 | if(ax_av.index == ax_av.samples) ax_av.index = 0; 45 | ax = ax_av.sum/ax_av.samples; 46 | 47 | ay_av.sum -= ay_av.memory[ay_av.index]; 48 | ay_av.memory[ay_av.index] = ay; 49 | ay_av.sum += ay_av.memory[ay_av.index]; 50 | ay_av.index++; 51 | if(ay_av.index == ay_av.samples) ay_av.index = 0; 52 | ay = ay_av.sum/ay_av.samples; 53 | 54 | az_av.sum -= az_av.memory[az_av.index]; 55 | az_av.memory[az_av.index] = az; 56 | az_av.sum += az_av.memory[az_av.index]; 57 | az_av.index++; 58 | if(az_av.index == az_av.samples) az_av.index = 0; 59 | az = az_av.sum/az_av.samples; 60 | 61 | /*Serial.print(ax); 62 | Serial.print(" "); 63 | Serial.print(ay); 64 | Serial.print(" "); 65 | Serial.println(az);*/ 66 | 67 | // x, y angles from gyroscope 68 | float gyr_roll = gx*t_delta*(PI/180.0); 69 | float gyr_pitch = gy*t_delta*(PI/180.0); 70 | 71 | // x, y angles from accelerometer 72 | //float acc_roll = atan2(ay, sqrt(pow(ax, 2) + pow(az, 2))) - roll_offset; 73 | //float acc_pitch = atan2(-1.0*ax, sqrt(pow(ay, 2) + pow(az, 2))) - pitch_offset; 74 | 75 | float acc_roll = atan2(ay, sqrt(pow(ax, 2) + pow(az, 2))) - roll_offset; 76 | float acc_pitch = atan2(-1.0*ax, sqrt(pow(ay, 2) + pow(az, 2))) - pitch_offset; 77 | 78 | acc_roll_av.sum -= acc_roll_av.memory[acc_roll_av.index]; 79 | acc_roll_av.memory[acc_roll_av.index] = acc_roll; 80 | acc_roll_av.sum += acc_roll_av.memory[acc_roll_av.index]; 81 | acc_roll_av.index++; 82 | if(acc_roll_av.index == acc_roll_av.samples) acc_roll_av.index = 0; 83 | 84 | acc_pitch_av.sum -= acc_pitch_av.memory[acc_pitch_av.index]; 85 | acc_pitch_av.memory[acc_pitch_av.index] = acc_pitch; 86 | acc_pitch_av.sum += acc_pitch_av.memory[acc_pitch_av.index]; 87 | acc_pitch_av.index++; 88 | if(acc_pitch_av.index == acc_pitch_av.samples) acc_pitch_av.index = 0; 89 | 90 | // x, y complementary filter for angles 91 | roll = 0.99*(roll + gyr_roll) + 0.01*acc_roll_av.sum/acc_roll_av.samples; 92 | pitch = 0.99*(pitch + gyr_pitch) + 0.01*acc_pitch_av.sum/acc_pitch_av.samples; 93 | 94 | // Get roll and pitch fot compass tilt compensation 95 | float mag_roll = pitch; 96 | float mag_pitch = -roll; 97 | 98 | // Magnetometer tilt ompensation 99 | float xh = mx*cos(mag_pitch) + my*sin(mag_roll)*sin(mag_pitch) + mz*cos(mag_roll)*sin(mag_pitch); 100 | float yh = my*cos(mag_roll) - mz*sin(mag_roll); 101 | 102 | // Yaw angle from magnetometer 103 | float mag_yaw = (atan2(yh, xh) - yaw_offset)*-1.0; 104 | if (mag_yaw < 0) { 105 | mag_yaw += 2 * PI; 106 | } 107 | if (mag_yaw > 2 * PI) { 108 | mag_yaw -= 2 * PI; 109 | } 110 | 111 | // Yaw angle from gyroscope 112 | float gyr_yaw = gz*t_delta*(PI/180.0); 113 | yaw += gyr_yaw; 114 | if (yaw < 0) { 115 | yaw += 2 * PI; 116 | } 117 | if (yaw > 2 * PI) { 118 | yaw -= 2 * PI; 119 | } 120 | 121 | // x, y complementary filter for angles 122 | yaw = 0.96*yaw + 0.04*mag_yaw; 123 | 124 | #if SHIFTED_YAW 125 | shifted_yaw = yaw - PI; 126 | #else 127 | shifted_yaw = yaw; 128 | #endif 129 | } 130 | void SensorFusion::fuse_sensors(float ax, float ay, float az, float gx, float gy, float gz) 131 | { 132 | // Get time delta between updates 133 | unsigned long current_time = millis(); 134 | float t_delta = ((float)current_time - (float)prev_time)/1000; 135 | prev_time = current_time; 136 | 137 | ax_av.sum -= ax_av.memory[ax_av.index]; 138 | ax_av.memory[ax_av.index] = ax; 139 | ax_av.sum += ax_av.memory[ax_av.index]; 140 | ax_av.index++; 141 | if(ax_av.index == ax_av.samples) ax_av.index = 0; 142 | ax = ax_av.sum/ax_av.samples; 143 | 144 | ay_av.sum -= ay_av.memory[ay_av.index]; 145 | ay_av.memory[ay_av.index] = ay; 146 | ay_av.sum += ay_av.memory[ay_av.index]; 147 | ay_av.index++; 148 | if(ay_av.index == ay_av.samples) ay_av.index = 0; 149 | ay = ay_av.sum/ay_av.samples; 150 | 151 | az_av.sum -= az_av.memory[az_av.index]; 152 | az_av.memory[az_av.index] = az; 153 | az_av.sum += az_av.memory[az_av.index]; 154 | az_av.index++; 155 | if(az_av.index == az_av.samples) az_av.index = 0; 156 | az = az_av.sum/az_av.samples; 157 | 158 | 159 | // x, y angles from gyroscope 160 | float gyr_roll = gx*t_delta*(PI/180.0); 161 | float gyr_pitch = gy*t_delta*(PI/180.0); 162 | 163 | float acc_roll = atan2(ay, sqrt(pow(ax, 2) + pow(az, 2))) - roll_offset; 164 | float acc_pitch = atan2(-1.0*ax, sqrt(pow(ay, 2) + pow(az, 2))) - pitch_offset; 165 | 166 | acc_roll_av.sum -= acc_roll_av.memory[acc_roll_av.index]; 167 | acc_roll_av.memory[acc_roll_av.index] = acc_roll; 168 | acc_roll_av.sum += acc_roll_av.memory[acc_roll_av.index]; 169 | acc_roll_av.index++; 170 | if(acc_roll_av.index == acc_roll_av.samples) acc_roll_av.index = 0; 171 | 172 | acc_pitch_av.sum -= acc_pitch_av.memory[acc_pitch_av.index]; 173 | acc_pitch_av.memory[acc_pitch_av.index] = acc_pitch; 174 | acc_pitch_av.sum += acc_pitch_av.memory[acc_pitch_av.index]; 175 | acc_pitch_av.index++; 176 | if(acc_pitch_av.index == acc_pitch_av.samples) acc_pitch_av.index = 0; 177 | 178 | // x, y complementary filter for angles 179 | roll = 0.99*(roll + gyr_roll) + 0.01*acc_roll_av.sum/acc_roll_av.samples; 180 | pitch = 0.99*(pitch + gyr_pitch) + 0.01*acc_pitch_av.sum/acc_pitch_av.samples; 181 | } 182 | 183 | float SensorFusion::get_roll() 184 | { 185 | return roll; 186 | } 187 | 188 | float SensorFusion::get_pitch() 189 | { 190 | return pitch; 191 | } 192 | 193 | float SensorFusion::get_yaw() 194 | { 195 | return shifted_yaw; 196 | } 197 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/SensorFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORFUSION_LIB 2 | #define SENSORFUSION_LIB 3 | 4 | #include 5 | #include 6 | #include "RollingMemory.h" 7 | 8 | #define SHIFTED_YAW 1 9 | 10 | class SensorFusion 11 | { 12 | public: 13 | float roll_offset; 14 | float pitch_offset; 15 | 16 | // Constructor/destructor 17 | void init(float initial_roll, float initial_pitch, float initial_yaw); 18 | //~SensorFusion(); 19 | 20 | void set_yaw_offset(float offset); 21 | void set_acc_offsets(float rollOffset, float pitchOffset); 22 | void fuse_sensors(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); 23 | void fuse_sensors(float ax, float ay, float az, float gx, float gy, float gz); 24 | float get_roll(); 25 | float get_pitch(); 26 | float get_yaw(); 27 | 28 | private: 29 | RollingMemory acc_roll_av; 30 | RollingMemory acc_pitch_av; 31 | RollingMemory ax_av; 32 | RollingMemory ay_av; 33 | RollingMemory az_av; 34 | unsigned long prev_time; 35 | float roll; 36 | float pitch; 37 | float yaw; // Mag target is the initial value at which the yaw will calibrate 38 | float shifted_yaw; // shofted so that range goes from -180 to 180 39 | float yaw_offset; 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-BMX055-CC1125-Multiwii/i2c_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_WRAPPER 2 | #define I2C_WRAPPER 3 | 4 | #include 5 | 6 | #define SUCCESS 0 7 | #define ADDR_ERR 1 8 | #define DATA_ERR 2 9 | 10 | static uint8_t i2cset(uint8_t address, uint8_t *data, uint8_t len, bool stp) 11 | { 12 | //address = (address<<1); 13 | Wire.beginTransmission(address); 14 | for(int i = 0; i < len; i++) { Wire.write(data[i]); } 15 | // This needs to return 0 16 | return Wire.endTransmission(stp); 17 | } 18 | 19 | static void i2cget(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) 20 | { 21 | uint8_t tx_data[1] = {reg}; 22 | i2cset(address, tx_data, 1, false); 23 | 24 | Wire.requestFrom(address, len); 25 | uint8_t i = 0; 26 | while(Wire.available()){ 27 | if(len == i) 28 | return; 29 | data[i++] = Wire.read(); 30 | } 31 | } 32 | #endif 33 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii/IMU.cpp: -------------------------------------------------------------------------------- 1 | #include "IMU.h" 2 | 3 | int16_t gyrFiltered[3]; 4 | 5 | typedef struct fp_vector { 6 | float X; 7 | float Y; 8 | float Z; 9 | } t_fp_vector_def; 10 | 11 | typedef union { 12 | float A[3]; 13 | t_fp_vector_def V; 14 | } t_fp_vector; 15 | 16 | // Rotate Estimated vector(s) with small angle approximation, according to the gyro data 17 | void rotateV(struct fp_vector *v, float *delta) 18 | { 19 | struct fp_vector v_tmp = *v; 20 | 21 | // This does a "proper" matrix rotation using gyro deltas without small-angle approximation 22 | float mat[3][3]; 23 | float cosx, sinx, cosy, siny, cosz, sinz; 24 | float coszcosx, sinzcosx, coszsinx, sinzsinx; 25 | 26 | cosx = cosf(delta[0]); 27 | sinx = sinf(delta[0]); 28 | cosy = cosf(delta[1]); 29 | siny = sinf(delta[1]); 30 | cosz = cosf(delta[2]); 31 | sinz = sinf(delta[2]); 32 | 33 | coszcosx = cosz * cosx; 34 | sinzcosx = sinz * cosx; 35 | coszsinx = sinx * cosz; 36 | sinzsinx = sinx * sinz; 37 | 38 | mat[0][0] = cosz * cosy; 39 | mat[0][1] = -cosy * sinz; 40 | mat[0][2] = siny; 41 | mat[1][0] = sinzcosx + (coszsinx * siny); 42 | mat[1][1] = coszcosx - (sinzsinx * siny); 43 | mat[1][2] = -sinx * cosy; 44 | mat[2][0] = (sinzsinx) - (coszcosx * siny); 45 | mat[2][1] = (coszsinx) + (sinzcosx * siny); 46 | mat[2][2] = cosy * cosx; 47 | 48 | v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0]; 49 | v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1]; 50 | v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; 51 | } 52 | 53 | 54 | //void getEstimatedAttitude(){ 55 | void getEstimatedAttitude(int16_t* acc, int16_t* gyr) 56 | { 57 | int32_t axis; 58 | int32_t accMag = 0; 59 | float deltaGyroAngle[3]; 60 | 61 | static t_fp_vector EstG = {0, 0, ACC_1G}; 62 | 63 | static uint32_t previousT; 64 | uint32_t currentT = micros(); 65 | uint32_t deltaT = currentT - previousT; 66 | float scale = deltaT * GYRO_SCALE; 67 | previousT = currentT; 68 | 69 | // Initialization 70 | for (axis = 0; axis < 3; axis++) { 71 | gyrSmooth[axis] = 0.7*gyrSmooth[axis] + 0.3*gyr[axis]; 72 | gyrFiltered[axis] = 0.95*gyrFiltered[axis] + 0.05*gyr[axis]; 73 | accSmooth[axis] = 0.99*accSmooth[axis] + 0.01*acc[axis]; 74 | 75 | accMag += (int32_t)accSmooth[axis] * accSmooth[axis]; 76 | 77 | // This could/shoule be done using filtered gyro 78 | //deltaGyroAngle[axis] = gyr[axis] * scale; 79 | deltaGyroAngle[axis] = gyrFiltered[axis] * scale; 80 | } 81 | 82 | /* 83 | Serial.print(accSmooth[0]); 84 | Serial.print(" "); 85 | Serial.print(accSmooth[1]); 86 | Serial.print(" "); 87 | Serial.println(accSmooth[2]); 88 | */ 89 | 90 | accMag = accMag * 100 / ((int32_t)ACC_1G * ACC_1G); 91 | 92 | rotateV(&EstG.V, deltaGyroAngle); 93 | 94 | if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) { 95 | for (axis = 0; axis < 3; axis++) 96 | EstG.A[axis] = EstG.A[axis]*0.99 + accSmooth[axis]*0.01; 97 | } 98 | 99 | float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians 100 | 101 | // Attitude of the estimated vector 102 | anglerad[0] = atan2f(EstG.V.Y, EstG.V.Z); 103 | anglerad[1] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); 104 | angle[0] = lrintf(anglerad[0] * (1800.0f / M_PI)); 105 | angle[1] = lrintf(anglerad[1] * (1800.0f / M_PI)); 106 | } 107 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii/IMU.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_H_ 2 | #define IMU_H_ 3 | 4 | #include 5 | #include "MPU.h" 6 | 7 | extern int16_t gyrSmooth[3]; 8 | extern int16_t accSmooth[3]; 9 | extern int16_t angle[2]; 10 | 11 | void getEstimatedAttitude(int16_t* acc, int16_t* gyr); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii/MPU.cpp: -------------------------------------------------------------------------------- 1 | #include "MPU.h" 2 | 3 | MPU::MPU(uint8_t address) 4 | { 5 | dev_address = address; 6 | } 7 | 8 | void MPU::mpu_write(uint8_t reg, uint8_t val) 9 | { 10 | uint8_t data[2] = {reg, val}; 11 | i2cset(dev_address, data, 2, true); 12 | } 13 | 14 | uint8_t MPU::mpu_single_read(uint8_t reg) 15 | { 16 | uint8_t data[1]; 17 | i2cget(dev_address, reg, data, 1); 18 | return data[0]; 19 | } 20 | 21 | void MPU::initialize() 22 | { 23 | 24 | mpu_write(0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 25 | delay(50); 26 | mpu_write(0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) 27 | mpu_write(0x1A, GYRO_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) 28 | mpu_write(0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec 29 | 30 | mpu_write(0x1C, 0x10); //ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec. 31 | 32 | uint8_t who_am_i = mpu_single_read(0x75); 33 | Serial.println(who_am_i, HEX); 34 | } 35 | 36 | void MPU::GYRO_Common() 37 | { 38 | static int16_t previousGyroADC[3] = {0,0,0}; 39 | static int32_t g[3]; 40 | uint8_t axis, tilt=0; 41 | 42 | if (calibratingG>0) { 43 | for (axis = 0; axis < 3; axis++) { 44 | if (calibratingG == 512) { // Reset g[axis] at start of calibration 45 | g[axis]=0; 46 | } 47 | g[axis] +=raw_gyr[axis]; // Sum up 512 readings 48 | gyroZero[axis]=g[axis]>>9; 49 | } 50 | calibratingG--; 51 | if(calibratingG == 0) 52 | { 53 | calibrationFinished = 1; 54 | } 55 | } 56 | 57 | for (axis = 0; axis < 3; axis++) { 58 | raw_gyr[axis] -= gyroZero[axis]; 59 | //anti gyro glitch, limit the variation between two consecutive readings 60 | raw_gyr[axis] = constrain(raw_gyr[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800); 61 | previousGyroADC[axis] = raw_gyr[axis]; 62 | } 63 | } 64 | 65 | void MPU::ACC_Common() 66 | { 67 | static int32_t a[3]; 68 | if (calibratingA>0) { 69 | calibratingA--; 70 | for (uint8_t axis = 0; axis < 3; axis++) { 71 | if (calibratingA == 511) a[axis]=0; // Reset a[axis] at start of calibration 72 | a[axis] +=raw_acc[axis]; // Sum up 512 readings 73 | accZero[axis] = a[axis]>>9; // Calculate average, only the last itteration where (calibratingA == 0) is relevant 74 | //global_conf.accZero[axis] = a[axis]>>9; // Calculate average, only the last itteration where (calibratingA == 0) is relevant 75 | } 76 | if (calibratingA == 0) { 77 | accZero[2] -= ACC_1G; // shift Z down by ACC_1G and store values in EEPROM at end of calibration 78 | 79 | int address = 0; 80 | 81 | EEPROM.put(address, accZero[0]); 82 | address += sizeof(int16_t); 83 | EEPROM.put(address, accZero[1]); 84 | address += sizeof(int16_t); 85 | EEPROM.put(address, accZero[2]); 86 | EEPROM.commit(); 87 | } 88 | } 89 | 90 | raw_acc[0] -= accZero[0]; 91 | raw_acc[1] -= accZero[1]; 92 | raw_acc[2] -= accZero[2]; 93 | } 94 | 95 | void MPU::Gyro_getADC () 96 | { 97 | uint8_t raw[6]; 98 | 99 | i2cget(dev_address, 0x43, raw, 6); 100 | 101 | raw_gyr[0] = (raw[2] <<8 | raw[3]); 102 | raw_gyr[0] = raw_gyr[0]>>2; 103 | raw_gyr[0] = -raw_gyr[0]; 104 | 105 | raw_gyr[1] = (raw[0] <<8 | raw[1]); 106 | raw_gyr[1] = raw_gyr[1]>>2; 107 | 108 | raw_gyr[2] = (raw[4] <<8 | raw[5]); 109 | raw_gyr[2] = raw_gyr[2]>>2; 110 | 111 | GYRO_Common(); 112 | } 113 | 114 | void MPU::ACC_getADC () 115 | { 116 | uint8_t raw[6]; 117 | 118 | i2cget(dev_address, 0x3B, raw, 6); 119 | 120 | raw_acc[0] = (raw[2] <<8 | raw[3]); 121 | //raw_acc[0] = raw_acc[0]>>3; 122 | raw_acc[0] = -raw_acc[0]; 123 | 124 | raw_acc[1] = (raw[0] <<8 | raw[1]); 125 | //raw_acc[1] = raw_acc[1]>>3; 126 | //raw_acc[1] = -raw_acc[1]; 127 | 128 | raw_acc[2] = (raw[4] <<8 | raw[5]); 129 | //raw_acc[2] = raw_acc[2]>>3; 130 | 131 | ACC_Common(); 132 | } 133 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii/MPU.h: -------------------------------------------------------------------------------- 1 | #ifndef MPU_H_ 2 | #define MPU_H_ 3 | 4 | #include 5 | //#include 6 | #include "i2c_wrapper.h" 7 | #include 8 | 9 | #define MPU6050_DEFAULT_ADDRESS 0x68 10 | //#define GYRO_DLPF_CFG 0 //Default settings LPF 256Hz/8000Hz sample 11 | //#define GYRO_DLPF_CFG 5 // 10Hz LPF 12 | #define GYRO_DLPF_CFG 6 // 5Hz LPF 13 | 14 | #define ACC_1G 512*8 15 | 16 | #define GYRO_SCALE (4 / 16.4 * PI / 180.0 / 1000000.0) //16.4 LSB = 1 deg/s 17 | 18 | class MPU 19 | { 20 | public: 21 | uint16_t calibratingG; 22 | uint16_t calibratingA; 23 | uint8_t calibrationFinished = 0; 24 | 25 | int16_t raw_acc[3]; 26 | int16_t raw_gyr[3]; 27 | 28 | // Public so EEPROM values can be stored here 29 | int16_t accZero[3]; 30 | 31 | MPU(uint8_t address = MPU6050_DEFAULT_ADDRESS); 32 | 33 | void initialize(); 34 | void Gyro_getADC (); 35 | void ACC_getADC (); 36 | 37 | private: 38 | uint8_t dev_address; 39 | 40 | int16_t gyroZero[3]; 41 | 42 | void GYRO_Common(); 43 | void ACC_Common(); 44 | 45 | void mpu_write(uint8_t reg, uint8_t val); 46 | uint8_t mpu_single_read(uint8_t reg); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /DEPRECATED/BrushedFlightController-RTOS-MPU6050-ESPNow-Multiwii/i2c_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_WRAPPER 2 | #define I2C_WRAPPER 3 | 4 | #include 5 | 6 | #define SUCCESS 0 7 | #define ADDR_ERR 1 8 | #define DATA_ERR 2 9 | 10 | static uint8_t i2cset(uint8_t address, uint8_t *data, uint8_t len, bool stp) 11 | { 12 | //address = (address<<1); 13 | Wire.beginTransmission(address); 14 | for(int i = 0; i < len; i++) { Wire.write(data[i]); } 15 | // This needs to return 0 16 | return Wire.endTransmission(stp); 17 | } 18 | 19 | static void i2cget(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) 20 | { 21 | uint8_t tx_data[1] = {reg}; 22 | i2cset(address, tx_data, 1, false); 23 | 24 | Wire.requestFrom(address, len); 25 | uint8_t i = 0; 26 | while(Wire.available()){ 27 | if(len == i) 28 | return; 29 | data[i++] = Wire.read(); 30 | } 31 | } 32 | #endif 33 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/BMX055.h: -------------------------------------------------------------------------------- 1 | #ifndef BMX055_LIB 2 | #define BMX055_LIB 3 | 4 | #include 5 | #include "math.h" 6 | #include "i2c_wrapper.h" 7 | 8 | #define DEBUG 0 9 | 10 | #define CALIBRATE_YAW 1 11 | #define USE_MAG_CALIBRATION 1 12 | #define USE_MAG_RAWDATA 0 13 | #define MAG_TARGET 3.14159// 1.57079632679f = pi/2 14 | 15 | // Accelerometer registers 16 | #define AM_CHIPID_REG 0x00 // Chip ud should be 0xFA 17 | #define AM_BGW_SOFT_RESET 0x14 // Reset register 18 | #define AM_PMU_RANGE 0x0F // G range 19 | #define AM_PMU_BW 0x10 // Bandwidth 20 | #define AM_OFC_CTRL 0x36 // Configuracion de Fast/slow Offset Compensation 21 | #define AM_OFC_SETTING 0x37 // Configuracion de Fast/slow Offset Compensation 22 | #define AM_OFC_OFFSET_X 0x38 // Offset value x, 8 bits 23 | #define AM_OFC_OFFSET_Y 0x39 // Offset value y, 8 bits 24 | #define AM_OFC_OFFSET_Z 0x3A // Offset value z, 8 bits 25 | #define AM_ACCD_X_LSB 0x02 // LSB Data for accelerometer in X 26 | #define AM_ACCD_Y_LSB 0x04 // LSB Data for accelerometer in Y 27 | #define AM_ACCD_Z_LSB 0x06 // LSB Data for accelerometer in Z 28 | 29 | // Gyroscope registers 30 | #define G_CHIPID_REG 0x00 // Should be 0x0F 31 | #define G_BGW_SOFT_RESET 0x14 // Reset register 32 | #define G_RANGE 0x0F // Range settings 33 | #define G_BW 0x10 // Bandwidth settings 34 | #define G_RATE_HBW 0x13 // Select (un)filtered output and shadowing enabling 35 | #define G_A_FOC 0x32 // Fast offset compensation settings 36 | #define G_OFC1 0x36 37 | #define G_OFC2 0x37 38 | #define G_OFC3 0x38 39 | #define G_OFC4 0x39 40 | #define G_TRIM_GP0 0x3A 41 | #define G_RATE_X_LSB 0x02 // LSB Data for gyroscope in X 42 | #define G_RATE_Y_LSB 0x04 // LSB Data for gyroscope in Y 43 | #define G_RATE_Z_LSB 0x06 // LSB Data for gyroscope in Z 44 | 45 | // Magnetometer registers 46 | #define MAG_CHIPID_REG 0x40 // Should be 0x32 47 | #define MAG_SOFT_RESET 0x4B // Reset register 48 | #define MAG_MODR 0x4C 49 | #define MAG_REPXY 0x51 50 | #define MAG_REPZ 0x52 51 | #define MAG_DATAX_LSB 0x42 // LSB Data for magnetometer in X 52 | #define MAG_DATAY_LSB 0x44 // LSB Data for magnetometer in Y 53 | #define MAG_DATAZ_LSB 0x46 // LSB Data for magnetometer in z 54 | #define MAG_RHALL_LSB 0x48 // LSB Data for magnetometer hall resistor 55 | 56 | // Magnetometer extended registers 57 | #define BMM150_DIG_X1 UINT8_C(0x5D) 58 | #define BMM150_DIG_Y1 UINT8_C(0x5E) 59 | #define BMM150_DIG_Z4_LSB UINT8_C(0x62) 60 | #define BMM150_DIG_Z4_MSB UINT8_C(0x63) 61 | #define BMM150_DIG_X2 UINT8_C(0x64) 62 | #define BMM150_DIG_Y2 UINT8_C(0x65) 63 | #define BMM150_DIG_Z2_LSB UINT8_C(0x68) 64 | #define BMM150_DIG_Z2_MSB UINT8_C(0x69) 65 | #define BMM150_DIG_Z1_LSB UINT8_C(0x6A) 66 | #define BMM150_DIG_Z1_MSB UINT8_C(0x6B) 67 | #define BMM150_DIG_XYZ1_LSB UINT8_C(0x6C) 68 | #define BMM150_DIG_XYZ1_MSB UINT8_C(0x6D) 69 | #define BMM150_DIG_Z3_LSB UINT8_C(0x6E) 70 | #define BMM150_DIG_Z3_MSB UINT8_C(0x6F) 71 | #define BMM150_DIG_XY2 UINT8_C(0x70) 72 | #define BMM150_DIG_XY1 UINT8_C(0x71) 73 | 74 | typedef struct { 75 | uint8_t chipid; 76 | uint16_t range; 77 | float res; 78 | float bandwidth; 79 | float sample_time; 80 | double x_offset; 81 | double y_offset; 82 | double z_offset; 83 | double x; 84 | double y; 85 | double z; 86 | }SensorVars; 87 | 88 | typedef struct { 89 | // trim values 90 | int8_t dig_x1; 91 | int8_t dig_y1; 92 | int8_t dig_x2; 93 | int8_t dig_y2; 94 | uint16_t dig_z1; 95 | int16_t dig_z2; 96 | int16_t dig_z3; 97 | int16_t dig_z4; 98 | uint8_t dig_xy1; 99 | int8_t dig_xy2; 100 | uint16_t dig_xyz1; 101 | 102 | // Configuration values 103 | uint8_t chipid; 104 | float res = 1.0/16.0; //uT or 1/1.6 milliGaus per LSB x*0.0625 = y*0.1 105 | uint8_t odr; 106 | String opmode; 107 | int repetitions_xy; 108 | int repetitions_z; 109 | 110 | 111 | // Magnetometer measurements 112 | double x_offset = -3.511194; 113 | double y_offset = -34.861936; 114 | double z_offset = -35.088470; 115 | double soft_iron[9] = {16.050133, 0.041485, -0.194715, 0.041485, 15.947023, 0.036212, -0.194715, 0.036212, 17.987555}; 116 | double yaw_offset = 0; 117 | double x; 118 | double y; 119 | double z; 120 | 121 | }MagnetometerVars; 122 | 123 | class BMX055 124 | { 125 | public: 126 | // Sensor addresses 127 | uint8_t Addr_Accel; 128 | uint8_t Addr_Gyro; 129 | uint8_t Addr_magnet; 130 | uint8_t mag_cal; 131 | 132 | // Constructor/destructor 133 | BMX055(uint8_t AddrAccel, uint8_t AddrGyro, uint8_t Addrmagnet, uint8_t magcal); 134 | ~BMX055(); 135 | 136 | // Accelerometer functions and struct 137 | SensorVars accelerometer; 138 | void acc_init(); 139 | void get_acc_data(); 140 | void get_acc_settings(); 141 | 142 | // Gyroscope functions and struct 143 | SensorVars gyroscope; 144 | int16_t ACC_1G; 145 | void gyr_init(); 146 | void get_gyr_data(); 147 | void get_gyr_settings(); 148 | 149 | // Magnetometer functions and struct 150 | MagnetometerVars magnetometer; 151 | void mag_init(); 152 | void mag_read_trim_registers(); 153 | void get_mag_data(); 154 | void get_mag_settings(); 155 | void mag_yaw_calibration(uint8_t samples); 156 | float mag_compensate_x(int16_t mag_data_x, uint16_t data_rhall); 157 | float mag_compensate_y(int16_t mag_data_y, uint16_t data_rhall); 158 | float mag_compensate_z(int16_t mag_data_z, uint16_t data_rhall); 159 | 160 | private: 161 | uint8_t acc_read(uint8_t reg); 162 | uint8_t acc_write(uint8_t reg, uint8_t data); 163 | uint8_t gyr_read(uint8_t reg); 164 | uint8_t gyr_write(uint8_t reg, uint8_t data); 165 | uint8_t mag_read(uint8_t reg); 166 | uint8_t mag_write(uint8_t reg, uint8_t data); 167 | int16_t twos_comp(uint16_t val, uint8_t bits); 168 | }; 169 | 170 | #endif 171 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/Definitions.h: -------------------------------------------------------------------------------- 1 | #ifndef DEFINITIONS_h 2 | #define DEFINITIONS_h 3 | 4 | #include "Arduino.h" 5 | 6 | // This allows to extern variables when included from other files and to initialize variables when included from the MAIN_EXECUTION file (*.ino file) 7 | #ifdef MAIN_EXECUTION 8 | #define EXTERN 9 | #define _INIT(x) = x 10 | #define INITIALIZE 11 | #else 12 | #define EXTERN extern 13 | # define _INIT(x) 14 | #endif 15 | 16 | /********* Global Macro definitions *************/ 17 | 18 | // EEPROM onyl holds 3 float values whcih represent 3 axis acc offsets 19 | #define EEPROM_SIZE sizeof(float)*3 20 | 21 | // BMX055 IMU addresses 22 | #define AM_DEV 0x19//0x18 23 | #define G_DEV 0x69//0x68 24 | #define MAG_DEV 0x13////0x10 25 | 26 | // HW Pins 27 | #define PWMA 32 28 | #define PWMB 33 29 | #define PWMC 14 30 | #define PWMD 12 31 | #define VBAT_ADC 27 32 | #define IMU_INTERRUPT 25 33 | #define RF_INTERRUPT 15 34 | #define LED_PIN 0 35 | 36 | // setting PWM properties 37 | #define FREQ 32000 // 70000 was working well 38 | #define RESOLUTION 10 39 | #define ledChannelA 0 40 | #define ledChannelB 1 41 | #define ledChannelC 2 42 | #define ledChannelD 3 43 | 44 | // Conversion for PWM generation 45 | #define MAX_PWM (1< 5 | #include "BMX055.h" 6 | #include "MS5611.h" 7 | #include 8 | 9 | #define CAL_SAMPLES 512 10 | #define BARO_CAL_SAMPLES 250 11 | 12 | extern BMX055 imu; 13 | extern uint8_t calibrate_acc; 14 | extern float acc_calibration[3]; 15 | extern float gyroLPF[3]; 16 | extern float roll; 17 | extern float pitch; 18 | extern float acczSmooth; 19 | 20 | extern MS5611 altimeter; 21 | extern uint8_t calibrate_alt; 22 | extern int32_t BaroPID; 23 | extern int32_t errorVelocityI; 24 | extern int32_t EstAlt; 25 | extern int32_t AltitudeSetpoint; 26 | 27 | void get_gyr_compensated_data(); 28 | void get_acc_compensated_data(); 29 | void get_gyr_lpf(); 30 | uint8_t get_baro_data(); 31 | float applyDeadband(float value, float deadband); 32 | void attitude_estimation(float dt); 33 | void acceleration_estimation(float dt); 34 | uint8_t altitude_estimation(); 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/MS5611.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611.cpp - Class file for the MS5611 Barometric Pressure & Temperature Sensor Arduino Library. 3 | 4 | Version: 1.0.0 5 | (c) 2014 Korneliusz Jarzebski 6 | www.jarzebski.pl 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the version 3 GNU General Public License as 10 | published by the Free Software Foundation. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | #include "Arduino.h" 22 | 23 | #include 24 | #include 25 | 26 | #include "MS5611.h" 27 | 28 | bool MS5611::begin(ms5611_osr_t osr) 29 | { 30 | 31 | reset(); 32 | 33 | setOversampling(osr); 34 | 35 | delay(100); 36 | 37 | readPROM(); 38 | 39 | return true; 40 | } 41 | 42 | // Set oversampling value 43 | void MS5611::setOversampling(ms5611_osr_t osr) 44 | { 45 | switch (osr) 46 | { 47 | case MS5611_ULTRA_LOW_POWER: 48 | ct = 1; 49 | break; 50 | case MS5611_LOW_POWER: 51 | ct = 2; 52 | break; 53 | case MS5611_STANDARD: 54 | ct = 3; 55 | break; 56 | case MS5611_HIGH_RES: 57 | ct = 5; 58 | break; 59 | case MS5611_ULTRA_HIGH_RES: 60 | ct = 10; 61 | break; 62 | } 63 | 64 | uosr = osr; 65 | } 66 | 67 | // Get oversampling value 68 | uint8_t MS5611::getOversampling(void) 69 | { 70 | //return (ms5611_osr_t)uosr; 71 | return ct; 72 | } 73 | 74 | void MS5611::reset(void) 75 | { 76 | Wire.beginTransmission(MS5611_ADDRESS); 77 | Wire.write(MS5611_CMD_RESET); 78 | Wire.endTransmission(); 79 | } 80 | 81 | void MS5611::readPROM(void) 82 | { 83 | for (uint8_t offset = 0; offset < 6; offset++) 84 | { 85 | fc[offset] = readRegister16(MS5611_CMD_READ_PROM + (offset * 2)); 86 | } 87 | } 88 | 89 | void MS5611::requestTemperature(void) 90 | { 91 | Wire.beginTransmission(MS5611_ADDRESS); 92 | Wire.write(MS5611_CMD_CONV_D2 + uosr); 93 | Wire.endTransmission(); 94 | } 95 | 96 | void MS5611::requestPressure(void) 97 | { 98 | Wire.beginTransmission(MS5611_ADDRESS); 99 | Wire.write(MS5611_CMD_CONV_D1 + uosr); 100 | Wire.endTransmission(); 101 | } 102 | 103 | uint32_t MS5611::readRawTemperature(void) 104 | { 105 | raw_temperature = readRegister24(MS5611_CMD_ADC_READ); 106 | return raw_temperature; 107 | //return readRegister24(MS5611_CMD_ADC_READ); 108 | } 109 | 110 | uint32_t MS5611::readRawPressure(void) 111 | { 112 | raw_pressure = readRegister24(MS5611_CMD_ADC_READ); 113 | return raw_pressure; 114 | //return readRegister24(MS5611_CMD_ADC_READ); 115 | } 116 | 117 | int32_t MS5611::readPressure(uint32_t rawTemperature, bool compensation) 118 | { 119 | uint32_t D1 = readRawPressure(); 120 | 121 | uint32_t D2 = rawTemperature; 122 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 123 | 124 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 125 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 126 | 127 | if (compensation) 128 | { 129 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 130 | 131 | OFF2 = 0; 132 | SENS2 = 0; 133 | 134 | if (TEMP < 2000) 135 | { 136 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 137 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 138 | } 139 | 140 | if (TEMP < -1500) 141 | { 142 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 143 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 144 | } 145 | 146 | OFF = OFF - OFF2; 147 | SENS = SENS - SENS2; 148 | } 149 | 150 | uint32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 151 | 152 | return P; 153 | } 154 | 155 | int32_t MS5611::readPressure(bool compensation) 156 | { 157 | requestPressure(); 158 | delay(ct); 159 | uint32_t D1 = readRawPressure(); 160 | 161 | requestTemperature(); 162 | delay(ct); 163 | uint32_t D2 = readRawTemperature(); 164 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 165 | 166 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 167 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 168 | 169 | if (compensation) 170 | { 171 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 172 | 173 | OFF2 = 0; 174 | SENS2 = 0; 175 | 176 | if (TEMP < 2000) 177 | { 178 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 179 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 180 | } 181 | 182 | if (TEMP < -1500) 183 | { 184 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 185 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 186 | } 187 | 188 | OFF = OFF - OFF2; 189 | SENS = SENS - SENS2; 190 | } 191 | 192 | uint32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 193 | 194 | return P; 195 | } 196 | 197 | void MS5611::calculatePressure(int32_t *pressure, int32_t *temperature) 198 | { 199 | uint32_t D1 = raw_pressure; 200 | 201 | uint32_t D2 = raw_temperature; 202 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 203 | 204 | int64_t OFF = (int64_t)fc[1] * 65536 + (int64_t)fc[3] * dT / 128; 205 | int64_t SENS = (int64_t)fc[0] * 32768 + (int64_t)fc[2] * dT / 256; 206 | 207 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 208 | 209 | OFF2 = 0; 210 | SENS2 = 0; 211 | 212 | if (TEMP < 2000) 213 | { 214 | OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2; 215 | SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4; 216 | } 217 | 218 | if (TEMP < -1500) 219 | { 220 | OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500)); 221 | SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2; 222 | } 223 | 224 | OFF = OFF - OFF2; 225 | SENS = SENS - SENS2; 226 | 227 | int32_t P = (D1 * SENS / 2097152 - OFF) / 32768; 228 | 229 | if (pressure) 230 | *pressure = P; 231 | if (temperature) 232 | *temperature = TEMP; 233 | } 234 | 235 | double MS5611::readTemperature(bool compensation) 236 | { 237 | uint32_t D2 = readRawTemperature(); 238 | int32_t dT = D2 - (uint32_t)fc[4] * 256; 239 | 240 | int32_t TEMP = 2000 + ((int64_t) dT * fc[5]) / 8388608; 241 | 242 | TEMP2 = 0; 243 | 244 | if (compensation) 245 | { 246 | if (TEMP < 2000) 247 | { 248 | TEMP2 = (dT * dT) / (2 << 30); 249 | } 250 | } 251 | 252 | TEMP = TEMP - TEMP2; 253 | 254 | return ((double)TEMP/100); 255 | } 256 | 257 | // Calculate altitude from Pressure & Sea level pressure 258 | double MS5611::getAltitude(double pressure, double seaLevelPressure) 259 | { 260 | return (44330.0f * (1.0f - pow((double)pressure / (double)seaLevelPressure, 0.1902949f))); 261 | } 262 | 263 | // Calculate sea level from Pressure given on specific altitude 264 | double MS5611::getSeaLevel(double pressure, double altitude) 265 | { 266 | return ((double)pressure / pow(1.0f - ((double)altitude / 44330.0f), 5.255f)); 267 | } 268 | 269 | // Read 16-bit from register (oops MSB, LSB) 270 | uint16_t MS5611::readRegister16(uint8_t reg) 271 | { 272 | uint16_t value; 273 | Wire.beginTransmission(MS5611_ADDRESS); 274 | Wire.write(reg); 275 | Wire.endTransmission(); 276 | 277 | Wire.beginTransmission(MS5611_ADDRESS); 278 | Wire.requestFrom(MS5611_ADDRESS, 2); 279 | while(!Wire.available()) {}; 280 | uint8_t vha = Wire.read(); 281 | uint8_t vla = Wire.read(); 282 | Wire.endTransmission(); 283 | 284 | value = vha << 8 | vla; 285 | 286 | return value; 287 | } 288 | 289 | // Read 24-bit from register (oops XSB, MSB, LSB) 290 | uint32_t MS5611::readRegister24(uint8_t reg) 291 | { 292 | uint32_t value; 293 | Wire.beginTransmission(MS5611_ADDRESS); 294 | Wire.write(reg); 295 | Wire.endTransmission(); 296 | 297 | Wire.beginTransmission(MS5611_ADDRESS); 298 | Wire.requestFrom(MS5611_ADDRESS, 3); 299 | while(!Wire.available()) {}; 300 | uint8_t vxa = Wire.read(); 301 | uint8_t vha = Wire.read(); 302 | uint8_t vla = Wire.read(); 303 | Wire.endTransmission(); 304 | 305 | value = ((int32_t)vxa << 16) | ((int32_t)vha << 8) | vla; 306 | 307 | return value; 308 | } 309 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/MS5611.h: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611.h - Header file for the MS5611 Barometric Pressure & Temperature Sensor Arduino Library. 3 | 4 | Version: 1.0.0 5 | (c) 2014 Korneliusz Jarzebski 6 | www.jarzebski.pl 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the version 3 GNU General Public License as 10 | published by the Free Software Foundation. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | */ 20 | 21 | #ifndef MS5611_h 22 | #define MS5611_h 23 | 24 | #include "Arduino.h" 25 | 26 | #define MS5611_ADDRESS (0x77) 27 | //#define MS5611_ADDRESS (0x76) 28 | 29 | #define MS5611_CMD_ADC_READ (0x00) 30 | #define MS5611_CMD_RESET (0x1E) 31 | #define MS5611_CMD_CONV_D1 (0x40) 32 | #define MS5611_CMD_CONV_D2 (0x50) 33 | #define MS5611_CMD_READ_PROM (0xA2) 34 | 35 | typedef enum 36 | { 37 | MS5611_ULTRA_HIGH_RES = 0x08, 38 | MS5611_HIGH_RES = 0x06, 39 | MS5611_STANDARD = 0x04, 40 | MS5611_LOW_POWER = 0x02, 41 | MS5611_ULTRA_LOW_POWER = 0x00 42 | } ms5611_osr_t; 43 | 44 | class MS5611 45 | { 46 | public: 47 | 48 | uint8_t ct; 49 | 50 | bool begin(ms5611_osr_t osr = MS5611_HIGH_RES); 51 | void requestTemperature(void); 52 | void requestPressure(void); 53 | uint32_t readRawTemperature(void); 54 | uint32_t readRawPressure(void); 55 | 56 | double readTemperature(bool compensation = false); 57 | int32_t readPressure(bool compensation = false); 58 | int32_t readPressure(uint32_t rawTemperature, bool compensation = false); 59 | void calculatePressure(int32_t *pressure, int32_t *temperature); 60 | double getAltitude(double pressure, double seaLevelPressure = 101325); 61 | double getSeaLevel(double pressure, double altitude); 62 | void setOversampling(ms5611_osr_t osr); 63 | uint8_t getOversampling(void); 64 | 65 | private: 66 | 67 | uint16_t fc[6]; 68 | uint8_t uosr; 69 | int32_t TEMP2; 70 | int64_t OFF2, SENS2; 71 | 72 | uint32_t raw_temperature; 73 | uint32_t raw_pressure; 74 | 75 | void reset(void); 76 | void readPROM(void); 77 | 78 | uint16_t readRegister16(uint8_t reg); 79 | uint32_t readRegister24(uint8_t reg); 80 | }; 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "PID.h" 2 | 3 | // Integration variables, constantly holding the sum of errors 4 | float yawErrorIntegral; 5 | float rollpitchErrorIntegral[2]; 6 | 7 | // Gyro derivative rolling average variables 8 | float prevDelta[2]; 9 | float prevPrevDelta[2]; 10 | float prevGyro[2] = {0,0}; 11 | 12 | /* 13 | * Attitude PID reinitialization 14 | */ 15 | void resetAttitudePID() 16 | { 17 | rollpitchErrorIntegral[ROLL] = 0; 18 | rollpitchErrorIntegral[PITCH] = 0; 19 | yawErrorIntegral = 0; 20 | } 21 | 22 | /* 23 | * Attitude PID calculation 24 | */ 25 | void calculateAttitudePID(float* setpoints) 26 | { 27 | 28 | // PITCH & ROLL PID control laws 29 | for(uint8_t axis=0;axis<2;axis++) { 30 | 31 | // ERROR claculation 32 | float error = setpoints[axis] - gyroLPF[axis]; 33 | 34 | // Error integral (constrained and limited to operate only when -156156) rollpitchErrorIntegral[axis] = 0; 37 | 38 | // Differential calculation (discrete derivative of gyro) 39 | float delta = gyroLPF[axis] - prevGyro[axis]; 40 | prevGyro[axis] = gyroLPF[axis]; 41 | 42 | // Rolling average of 3 samples (similar to LPF) on differential term 43 | float deltaLPF = (delta + prevDelta[axis] + prevPrevDelta[axis])/3.0; 44 | prevPrevDelta[axis] = prevDelta[axis]; 45 | prevDelta[axis] = delta; 46 | 47 | // PID laplace = Kp*(S-G) + Ki*(S-G)/s - Kd*(0-G)*s 48 | attitudePID[axis] = rate_rollpitch_kp * error + rate_rollpitch_ki * rollpitchErrorIntegral[axis] - rate_rollpitch_kd * deltaLPF; 49 | 50 | } 51 | 52 | //YAW PI control law 53 | // ERROR claculation 54 | float error = setpoints[YAW] - gyroLPF[YAW]; 55 | 56 | // Error integral (constrained and limited to operate only when -50 50) yawErrorIntegral = 0; 59 | 60 | // PI laplace = Kp*(S-G) + Ki*(S-G)/s 61 | attitudePID[YAW] = rate_yaw_kp * error + rate_yaw_ki * yawErrorIntegral; 62 | } 63 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_h 2 | #define PID_h 3 | 4 | #include "Arduino.h" 5 | #include "Estimates.h" 6 | #include "Definitions.h" 7 | 8 | /* 9 | * Attitude PID reinitialization 10 | * This functions resets time-dependant variables like integrals 11 | * When PID starts operating, integral initial conditions need to be 0 to avoid incorrect error accumulation 12 | */ 13 | void resetAttitudePID(); 14 | 15 | /* 16 | * Attitude PID calculation 17 | * This function calculates control laws for yaw, pitch and roll ased on the provided setpoints 18 | * Integral terms are constrained and ignored when drone is far from hovering point, this prevents excessive error accumulation 19 | * Derivative term is just the derivtive of the controlled variables, rather than the derivative of the error. This allos smoother D term on the PID 20 | */ 21 | void calculateAttitudePID(float* setpoints); 22 | 23 | // PID control laws get stored here 24 | extern float attitudePID[3]; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /ESP32BrushedFlightController/i2c_wrapper.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_WRAPPER 2 | #define I2C_WRAPPER 3 | 4 | #include 5 | 6 | #define SUCCESS 0 7 | #define ADDR_ERR 1 8 | #define DATA_ERR 2 9 | 10 | static uint8_t i2cset(uint8_t address, uint8_t *data, uint8_t len, bool stp) 11 | { 12 | //address = (address<<1); 13 | Wire.beginTransmission(address); 14 | for(int i = 0; i < len; i++) { Wire.write(data[i]); } 15 | // This needs to return 0 16 | return Wire.endTransmission(stp); 17 | } 18 | 19 | static void i2cget(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len) 20 | { 21 | uint8_t tx_data[1] = {reg}; 22 | i2cset(address, tx_data, 1, false); 23 | 24 | Wire.requestFrom(address, len); 25 | uint8_t i = 0; 26 | while(Wire.available()){ 27 | if(len == i) 28 | return; 29 | data[i++] = Wire.read(); 30 | } 31 | } 32 | #endif 33 | -------------------------------------------------------------------------------- /FlowDiagram-RTOS.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davidoises/FlightController/7bb12fd073a16ef1ae07648538a225fe0c13c74b/FlowDiagram-RTOS.jpg -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/ArduinoPS2_SimpleTest/ArduinoPS2_SimpleTest.ino: -------------------------------------------------------------------------------- 1 | #include "PS2X_lib.h" 2 | 3 | PS2X ps2x; // create PS2 Controller Class 4 | 5 | //right now, the library does NOT support hot pluggable controllers, meaning 6 | //you must always either restart your Arduino after you connect the controller, 7 | //or call config_gamepad(pins) again after connecting the controller. 8 | 9 | int error = 0; 10 | byte type = 0; 11 | byte vibrate = 0; 12 | 13 | void setup(){ 14 | 15 | Serial.begin(115200); 16 | 17 | delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it 18 | 19 | //CHANGES for v1.6 HERE!!! **************PAY ATTENTION************* 20 | 21 | //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error 22 | error = ps2x.config_gamepad(13, 11, 10, 12, true, true); 23 | } 24 | 25 | void loop() { 26 | /* You must Read Gamepad to get new values and set vibration values 27 | ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255) 28 | if you don't enable the rumble, use ps2x.read_gamepad(); with no values 29 | You should call this at least once a second 30 | */ 31 | if(error == 1) //skip loop if no controller found 32 | return; 33 | 34 | ps2x.read_gamepad(); 35 | 36 | if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed 37 | Serial.println("Start is being held"); 38 | if(ps2x.Button(PSB_SELECT)) 39 | Serial.println("Select is being held"); 40 | 41 | if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed 42 | Serial.print("Up held this hard: "); 43 | Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC); 44 | } 45 | if(ps2x.Button(PSB_PAD_RIGHT)){ 46 | Serial.print("Right held this hard: "); 47 | Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC); 48 | } 49 | if(ps2x.Button(PSB_PAD_LEFT)){ 50 | Serial.print("LEFT held this hard: "); 51 | Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC); 52 | } 53 | if(ps2x.Button(PSB_PAD_DOWN)){ 54 | Serial.print("DOWN held this hard: "); 55 | Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC); 56 | } 57 | 58 | vibrate = ps2x.Analog(PSAB_CROSS); //this will set the large motor vibrate speed based on how hard you press the blue (X) button 59 | if (ps2x.NewButtonState()) { //will be TRUE if any button changes state (on to off, or off to on) 60 | if(ps2x.Button(PSB_L3)) 61 | Serial.println("L3 pressed"); 62 | if(ps2x.Button(PSB_R3)) 63 | Serial.println("R3 pressed"); 64 | if(ps2x.Button(PSB_L2)) 65 | Serial.println("L2 pressed"); 66 | if(ps2x.Button(PSB_R2)) 67 | Serial.println("R2 pressed"); 68 | if(ps2x.Button(PSB_TRIANGLE)) 69 | Serial.println("Triangle pressed"); 70 | } 71 | 72 | if(ps2x.ButtonPressed(PSB_CIRCLE)) //will be TRUE if button was JUST pressed 73 | Serial.println("Circle just pressed"); 74 | if(ps2x.NewButtonState(PSB_CROSS)) //will be TRUE if button was JUST pressed OR released 75 | Serial.println("X just changed"); 76 | if(ps2x.ButtonReleased(PSB_SQUARE)) //will be TRUE if button was JUST released 77 | Serial.println("Square just released"); 78 | 79 | if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE 80 | //Serial.print("Stick Values:"); 81 | Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX 82 | Serial.print(","); 83 | Serial.print(ps2x.Analog(PSS_LX), DEC); 84 | Serial.print(","); 85 | Serial.print(ps2x.Analog(PSS_RY), DEC); 86 | Serial.print(","); 87 | Serial.println(ps2x.Analog(PSS_RX), DEC); 88 | } 89 | 90 | delay(50); 91 | } 92 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/ArduinoPS2_SimpleTest/PS2X_lib.h: -------------------------------------------------------------------------------- 1 | /****************************************************************** 2 | * Super amazing PS2 controller Arduino Library v1.8 3 | * details and example sketch: 4 | * http://www.billporter.info/?p=240 5 | * 6 | * Original code by Shutter on Arduino Forums 7 | * 8 | * Revamped, made into lib by and supporting continued development: 9 | * Bill Porter 10 | * www.billporter.info 11 | * 12 | * Contributers: 13 | * Eric Wetzel (thewetzel@gmail.com) 14 | * Kurt Eckhardt 15 | * 16 | * Lib version history 17 | * 0.1 made into library, added analog stick support. 18 | * 0.2 fixed config_gamepad miss-spelling 19 | * added new functions: 20 | * NewButtonState(); 21 | * NewButtonState(unsigned int); 22 | * ButtonPressed(unsigned int); 23 | * ButtonReleased(unsigned int); 24 | * removed 'PS' from beginning of ever function 25 | * 1.0 found and fixed bug that wasn't configuring controller 26 | * added ability to define pins 27 | * added time checking to reconfigure controller if not polled enough 28 | * Analog sticks and pressures all through 'ps2x.Analog()' function 29 | * added: 30 | * enableRumble(); 31 | * enablePressures(); 32 | * 1.1 33 | * added some debug stuff for end user. Reports if no controller found 34 | * added auto-increasing sentence delay to see if it helps compatibility. 35 | * 1.2 36 | * found bad math by Shutter for original clock. Was running at 50kHz, not the required 500kHz. 37 | * fixed some of the debug reporting. 38 | * 1.3 39 | * Changed clock back to 50kHz. CuriousInventor says it's suppose to be 500kHz, but doesn't seem to work for everybody. 40 | * 1.4 41 | * Removed redundant functions. 42 | * Fixed mode check to include two other possible modes the controller could be in. 43 | * Added debug code enabled by compiler directives. See below to enable debug mode. 44 | * Added button definitions for shapes as well as colors. 45 | * 1.41 46 | * Some simple bug fixes 47 | * Added Keywords.txt file 48 | * 1.5 49 | * Added proper Guitar Hero compatibility 50 | * Fixed issue with DEBUG mode, had to send serial at once instead of in bits 51 | * 1.6 52 | * Changed config_gamepad() call to include rumble and pressures options 53 | * This was to fix controllers that will only go into config mode once 54 | * Old methods should still work for backwards compatibility 55 | * 1.7 56 | * Integrated Kurt's fixes for the interrupts messing with servo signals 57 | * Reorganized directory so examples show up in Arduino IDE menu 58 | * 1.8 59 | * Added Arduino 1.0 compatibility. 60 | * 1.9 61 | * Kurt - Added detection and recovery from dropping from analog mode, plus 62 | * integrated Chipkit (pic32mx...) support 63 | * 64 | * 65 | * 66 | *This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or(at your option) any later version. 67 | This program is distributed in the hope that it will be useful, 68 | but WITHOUT ANY WARRANTY; without even the implied warranty of 69 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 70 | GNU General Public License for more details. 71 | 72 | * 73 | ******************************************************************/ 74 | 75 | // $$$$$$$$$$$$ DEBUG ENABLE SECTION $$$$$$$$$$$$$$$$ 76 | // to debug ps2 controller, uncomment these two lines to print out debug to uart 77 | //#define PS2X_DEBUG 78 | //#define PS2X_COM_DEBUG 79 | 80 | #ifndef PS2X_lib_h 81 | #define PS2X_lib_h 82 | 83 | #if ARDUINO > 22 84 | #include "Arduino.h" 85 | #else 86 | #include "WProgram.h" 87 | #endif 88 | 89 | #include 90 | #include 91 | #include 92 | #ifdef __AVR__ 93 | // AVR 94 | #include 95 | #define CTRL_CLK 4 96 | #define CTRL_BYTE_DELAY 3 97 | #else 98 | #ifdef ESP8266 99 | #define CTRL_CLK 5 100 | #define CTRL_CLK_HIGH 5 101 | #define CTRL_BYTE_DELAY 18 102 | #else 103 | // Pic32... 104 | #include 105 | #define CTRL_CLK 5 106 | #define CTRL_CLK_HIGH 5 107 | #define CTRL_BYTE_DELAY 4 108 | #endif 109 | #endif 110 | 111 | //These are our button constants 112 | #define PSB_SELECT 0x0001 113 | #define PSB_L3 0x0002 114 | #define PSB_R3 0x0004 115 | #define PSB_START 0x0008 116 | #define PSB_PAD_UP 0x0010 117 | #define PSB_PAD_RIGHT 0x0020 118 | #define PSB_PAD_DOWN 0x0040 119 | #define PSB_PAD_LEFT 0x0080 120 | #define PSB_L2 0x0100 121 | #define PSB_R2 0x0200 122 | #define PSB_L1 0x0400 123 | #define PSB_R1 0x0800 124 | #define PSB_GREEN 0x1000 125 | #define PSB_RED 0x2000 126 | #define PSB_BLUE 0x4000 127 | #define PSB_PINK 0x8000 128 | #define PSB_TRIANGLE 0x1000 129 | #define PSB_CIRCLE 0x2000 130 | #define PSB_CROSS 0x4000 131 | #define PSB_SQUARE 0x8000 132 | 133 | //Guitar button constants 134 | #define UP_STRUM 0x0010 135 | #define DOWN_STRUM 0x0040 136 | #define LEFT_STRUM 0x0080 137 | #define RIGHT_STRUM 0x0020 138 | #define STAR_POWER 0x0100 139 | #define GREEN_FRET 0x0200 140 | #define YELLOW_FRET 0x1000 141 | #define RED_FRET 0x2000 142 | #define BLUE_FRET 0x4000 143 | #define ORANGE_FRET 0x8000 144 | #define WHAMMY_BAR 8 145 | 146 | //These are stick values 147 | #define PSS_RX 5 148 | #define PSS_RY 6 149 | #define PSS_LX 7 150 | #define PSS_LY 8 151 | 152 | //These are analog buttons 153 | #define PSAB_PAD_RIGHT 9 154 | #define PSAB_PAD_UP 11 155 | #define PSAB_PAD_DOWN 12 156 | #define PSAB_PAD_LEFT 10 157 | #define PSAB_L2 19 158 | #define PSAB_R2 20 159 | #define PSAB_L1 17 160 | #define PSAB_R1 18 161 | #define PSAB_GREEN 13 162 | #define PSAB_RED 14 163 | #define PSAB_BLUE 15 164 | #define PSAB_PINK 16 165 | #define PSAB_TRIANGLE 13 166 | #define PSAB_CIRCLE 14 167 | #define PSAB_CROSS 15 168 | #define PSAB_SQUARE 16 169 | 170 | #define SET(x,y) (x|=(1<. 32 | */ 33 | #include "PsxLib.h" 34 | 35 | static byte enter_config[]={0x01,0x43,0x00,0x01,0x00}; 36 | static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00}; 37 | static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00}; 38 | static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A}; 39 | static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01}; 40 | static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A}; 41 | 42 | Psx::Psx() { }; // Class Psx 43 | 44 | byte Psx::shift(byte _dataOut) { // Does the actual shifting, both in and out simultaneously 45 | _dataIn = 0; 46 | 47 | for (byte i = 0; i < 8; i++) { 48 | 49 | if ( _dataOut & (1 << i) ) 50 | digitalWrite(_cmndPin, HIGH); // Writes out the _dataOut bits 51 | else 52 | digitalWrite(_cmndPin, LOW); 53 | 54 | digitalWrite(_clockPin, LOW); 55 | delayMicroseconds(_delay); 56 | 57 | if ( digitalRead(_dataPin) ) { // Reads the data pin 58 | _dataIn |= _BV(i); // Giuseppe Shift the read into _dataIn (align to the others libs) 59 | } 60 | 61 | digitalWrite(_clockPin, HIGH); 62 | delayMicroseconds(_delay); 63 | } 64 | return _dataIn; 65 | } 66 | 67 | 68 | void Psx::setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay) { 69 | pinMode(dataPin, INPUT_PULLUP); 70 | //digitalWrite(dataPin, HIGH); // Turn on internal pull-up 71 | pinMode(attPin, OUTPUT); 72 | digitalWrite(_attPin, HIGH); 73 | pinMode(cmndPin, OUTPUT); 74 | pinMode(clockPin, OUTPUT); 75 | digitalWrite(_clockPin, HIGH); 76 | 77 | _dataPin = dataPin; 78 | _cmndPin = cmndPin; 79 | _attPin = attPin; 80 | _clockPin = clockPin; 81 | _delay = delay; 82 | 83 | delayMicroseconds(1000000); 84 | 85 | delayMicroseconds(1000000); 86 | 87 | 88 | 89 | do 90 | { 91 | sendCommandString(enter_config, sizeof(enter_config)); //start config run 92 | sendCommandString(set_mode, sizeof(set_mode)); 93 | sendCommandString(exit_config, sizeof(exit_config)); 94 | 95 | read(); 96 | Serial.println(_mode, HEX); // Shoujld be -x73 97 | } 98 | while(_mode & 0xf0 != 0x70); 99 | } 100 | 101 | 102 | uint32_t Psx::read() { 103 | digitalWrite(_attPin, LOW); 104 | 105 | shift(0x01); 106 | _mode = shift(0x42); 107 | shift(0xFF); 108 | 109 | _data1 = ~shift(0xFF); 110 | _data2 = ~shift(0xFF); 111 | byte _data3 = shift(0xFF); 112 | byte _data4 = shift(0xFF); 113 | byte _data5 = shift(0xFF); 114 | byte _data6 = shift(0xFF); 115 | 116 | digitalWrite(_attPin, HIGH); 117 | 118 | _last_buttons = _buttonsData; // Giuseppe: Save Old Value 119 | 120 | _buttonsData = (_data2 << 8) | _data1; 121 | _sticksData = (_data6 << 24) | (_data5 << 16) | (_data4 << 8) | _data3; 122 | 123 | return _buttonsData; 124 | } 125 | 126 | bool Psx::button(unsigned int button) { 127 | return ((_buttonsData & button) > 0); 128 | } 129 | 130 | bool Psx::newButtonState() { 131 | return ((_last_buttons ^ _buttonsData) > 0); 132 | } 133 | 134 | bool Psx::buttonNewState(unsigned int button) { 135 | return (((_last_buttons ^ _buttonsData) & button) > 0); 136 | } 137 | 138 | byte Psx::analog(byte button) { 139 | return (uint8_t) (_sticksData >> ((button)*8)); 140 | } 141 | 142 | void Psx::sendCommandString(byte string[], byte len) { 143 | 144 | digitalWrite(_attPin, LOW); 145 | 146 | for (int y=0; y < len; y++) 147 | { 148 | shift(string[y]); 149 | } 150 | digitalWrite(_attPin, HIGH); 151 | } 152 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/ESP32PS2_SimpleTest/PsxLib.h: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Library (PsxLib.h) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Cable side connected with two R10K with Clock and Data Pins. 19 | 20 | Controller protocol implemented using Andrew J McCubbin's analysis. 21 | http://www.gamesx.com/controldata/psxcont/psxcont.htm 22 | 23 | Shift command is based on tutorial examples for ShiftIn and ShiftOut 24 | functions both written by Carlyn Maw and Tom Igoe 25 | http://www.arduino.cc/en/Tutorial/ShiftIn 26 | http://www.arduino.cc/en/Tutorial/ShiftOut 27 | 28 | This program is free software: you can redistribute it and/or modify 29 | it under the terms of the GNU General Public License as published by 30 | the Free Software Foundation, either version 3 of the License, or 31 | (at your option) any later version. 32 | 33 | This program is distributed in the hope that it will be useful, 34 | but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | GNU General Public License for more details. 37 | 38 | You should have received a copy of the GNU General Public License 39 | along with this program. If not, see . 40 | */ 41 | 42 | 43 | #ifndef PSXLIB_H 44 | #define PSXLIB_H 45 | 46 | #if ARDUINO > 22 47 | #include "Arduino.h" 48 | #else 49 | #include "WProgram.h" 50 | #endif 51 | 52 | 53 | //These are our button constants 54 | #define PSB_IDLE 0x0000 55 | #define PSB_SELECT 0x0001 56 | #define PSB_START 0x0008 57 | #define PSB_L3 0x0002 58 | #define PSB_R3 0x0004 59 | #define PSB_L2 0x0100 60 | #define PSB_R2 0x0200 61 | #define PSB_L1 0x0400 62 | #define PSB_R1 0x0800 63 | #define PSB_GREEN 0x1000 64 | #define PSB_RED 0x2000 65 | #define PSB_BLUE 0x4000 66 | #define PSB_PINK 0x8000 67 | 68 | // Left Keys Pad and Stiks 69 | #define PSB_PAD_UP 0x0010 70 | #define PSB_PAD_RIGHT 0x0020 71 | #define PSB_PAD_DOWN 0x0040 72 | #define PSB_PAD_LEFT 0x0080 73 | 74 | #define PSB_LSTIK_LEFT 0x0010 75 | #define PSB_LSTIK_DOWN 0x0020 76 | #define PSB_LSTIK_RIGHT 0x0040 77 | #define PSB_LSTIK_UP 0x0080 78 | 79 | // Right Keys Pad and Stiks 80 | #define PSB_TRIANGLE 0x1000 81 | #define PSB_CIRCLE 0x2000 82 | #define PSB_CROSS 0x4000 83 | #define PSB_SQUARE 0x8000 84 | 85 | #define PSB_RSTIK_LEFT 0x1000 86 | #define PSB_RSTIK_DOWN 0x2000 87 | #define PSB_RSTIK_RIGHT 0x4000 88 | #define PSB_RSTIK_UP 0x8000 89 | 90 | class Psx { 91 | public: 92 | Psx(); 93 | void setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay); 94 | // Data Pin #, CMND Pin #, ATT Pin #, CLK Pin #, Delay 95 | // Delay is how long the clock goes without changing state in Microseconds. 96 | // It can be lowered to increase response, but if it is too low it may cause 97 | // glitches and have some keys spill over with false-positives. 98 | // A regular PSX controller works fine at 50 uSeconds. 99 | 100 | uint32_t read(); // Returns the status of the button presses in an unsignd int. 101 | // The value returned corresponds to each key as defined above. 102 | bool newButtonState(); // Giuseppe: will be true if any button was pressed 103 | bool buttonNewState(unsigned int); // Giuseppe: will be true if button changed state 104 | bool button(unsigned int); // Giuseppe: will be TRUE if button is being pressed 105 | byte analog(byte); 106 | 107 | private: 108 | void sendCommandString(byte*, byte); 109 | byte shift(byte _dataOut); 110 | 111 | byte _dataPin; 112 | byte _cmndPin; 113 | byte _attPin; 114 | byte _clockPin; 115 | 116 | byte _delay; 117 | boolean _temp; 118 | byte _dataIn; 119 | 120 | byte _mode; 121 | byte _data1; 122 | byte _data2; 123 | uint32_t _sticksData; 124 | uint32_t _buttonsData; 125 | uint32_t _last_buttons; // Giuseppe: Last Button Pressed 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/JoystickTests/JoystickTests.ino: -------------------------------------------------------------------------------- 1 | float jx = 1500; 2 | float jy = 1500; 3 | 4 | float mapJoystickValues(int val, int middle_point, float lower_output, float upper_output) 5 | { 6 | float output_middle_point = (lower_output + upper_output)/2; 7 | float temp = 0; 8 | if(val < middle_point) 9 | { 10 | temp = map(val, 0, middle_point, lower_output, output_middle_point); 11 | } 12 | else 13 | { 14 | temp = map(val, middle_point, 4095, output_middle_point, upper_output); 15 | } 16 | return temp; 17 | } 18 | 19 | void setup() { 20 | // put your setup code here, to run once: 21 | Serial.begin(115200); 22 | 23 | } 24 | 25 | void loop() { 26 | float x = mapJoystickValues(analogRead(14), 1907, 1000.0, 2000.0); 27 | float y = mapJoystickValues(analogRead(12), 1843, 1000.0, 2000.0); 28 | 29 | jx = 0.7*jx + 0.3*x; 30 | jy = 0.7*jy + 0.3*y; 31 | 32 | // put your main code here, to run repeatedly: 33 | Serial.print(jx); //X 34 | Serial.print(" "); 35 | Serial.println(jy); //Y 36 | delay(40); 37 | } 38 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2X_Example/PS2X_Example.ino: -------------------------------------------------------------------------------- 1 | #include "PS2X_lib.h" //for v1.6 2 | 3 | /****************************************************************** 4 | * set pins connected to PS2 controller: 5 | * - 1e column: original 6 | * - 2e colmun: Stef? 7 | * replace pin numbers by the ones you use 8 | ******************************************************************/ 9 | #define PS2_DAT 13 //14 10 | #define PS2_CMD 11 //15 11 | #define PS2_SEL 10 //16 12 | #define PS2_CLK 12 //17 13 | 14 | /****************************************************************** 15 | * select modes of PS2 controller: 16 | * - pressures = analog reading of push-butttons 17 | * - rumble = motor rumbling 18 | * uncomment 1 of the lines for each mode selection 19 | ******************************************************************/ 20 | //#define pressures true 21 | #define pressures false 22 | //#define rumble true 23 | #define rumble false 24 | 25 | PS2X ps2x; // create PS2 Controller Class 26 | 27 | //right now, the library does NOT support hot pluggable controllers, meaning 28 | //you must always either restart your Arduino after you connect the controller, 29 | //or call config_gamepad(pins) again after connecting the controller. 30 | 31 | int error = 0; 32 | byte type = 0; 33 | byte vibrate = 0; 34 | 35 | void setup(){ 36 | 37 | Serial.begin(115200); 38 | 39 | delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it 40 | 41 | //CHANGES for v1.6 HERE!!! **************PAY ATTENTION************* 42 | 43 | /*#ifdef ESP32 || ESP8266 44 | Serial.println("ESP32"); 45 | #endif*/ 46 | 47 | //pinMode(14, INPUT_PULLUP); 48 | //pinMode(12, INPUT_PULLUP); 49 | 50 | //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error 51 | //error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble); 52 | //error = ps2x.config_gamepad(13, 11, 10, 12, true, true); 53 | error = ps2x.config_gamepad(32, 27, 25, 26, true, true); 54 | 55 | if(error == 0){ 56 | Serial.print("Found Controller, configured successful "); 57 | Serial.print("pressures = "); 58 | if (pressures) 59 | Serial.println("true "); 60 | else 61 | Serial.println("false"); 62 | Serial.print("rumble = "); 63 | if (rumble) 64 | Serial.println("true)"); 65 | else 66 | Serial.println("false"); 67 | Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;"); 68 | Serial.println("holding L1 or R1 will print out the analog stick values."); 69 | Serial.println("Note: Go to www.billporter.info for updates and to report bugs."); 70 | } 71 | else if(error == 1) 72 | Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips"); 73 | 74 | else if(error == 2) 75 | Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips"); 76 | 77 | else if(error == 3) 78 | Serial.println("Controller refusing to enter Pressures mode, may not support it. "); 79 | 80 | // Serial.print(ps2x.Analog(1), HEX); 81 | 82 | type = ps2x.readType(); 83 | switch(type) { 84 | case 0: 85 | Serial.print("Unknown Controller type found "); 86 | break; 87 | case 1: 88 | Serial.print("DualShock Controller found "); 89 | break; 90 | case 2: 91 | Serial.print("GuitarHero Controller found "); 92 | break; 93 | case 3: 94 | Serial.print("Wireless Sony DualShock Controller found "); 95 | break; 96 | } 97 | } 98 | 99 | void loop() { 100 | /* You must Read Gamepad to get new values and set vibration values 101 | ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255) 102 | if you don't enable the rumble, use ps2x.read_gamepad(); with no values 103 | You should call this at least once a second 104 | */ 105 | if(error == 1) //skip loop if no controller found 106 | return; 107 | 108 | if(type == 2){ //Guitar Hero Controller 109 | ps2x.read_gamepad(); //read controller 110 | 111 | if(ps2x.ButtonPressed(GREEN_FRET)) 112 | Serial.println("Green Fret Pressed"); 113 | if(ps2x.ButtonPressed(RED_FRET)) 114 | Serial.println("Red Fret Pressed"); 115 | if(ps2x.ButtonPressed(YELLOW_FRET)) 116 | Serial.println("Yellow Fret Pressed"); 117 | if(ps2x.ButtonPressed(BLUE_FRET)) 118 | Serial.println("Blue Fret Pressed"); 119 | if(ps2x.ButtonPressed(ORANGE_FRET)) 120 | Serial.println("Orange Fret Pressed"); 121 | 122 | if(ps2x.ButtonPressed(STAR_POWER)) 123 | Serial.println("Star Power Command"); 124 | 125 | if(ps2x.Button(UP_STRUM)) //will be TRUE as long as button is pressed 126 | Serial.println("Up Strum"); 127 | if(ps2x.Button(DOWN_STRUM)) 128 | Serial.println("DOWN Strum"); 129 | 130 | if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed 131 | Serial.println("Start is being held"); 132 | if(ps2x.Button(PSB_SELECT)) 133 | Serial.println("Select is being held"); 134 | 135 | if(ps2x.Button(ORANGE_FRET)) { // print stick value IF TRUE 136 | Serial.print("Wammy Bar Position:"); 137 | Serial.println(ps2x.Analog(WHAMMY_BAR), DEC); 138 | } 139 | } 140 | else { //DualShock Controller 141 | ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed 142 | 143 | if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed 144 | Serial.println("Start is being held"); 145 | if(ps2x.Button(PSB_SELECT)) 146 | Serial.println("Select is being held"); 147 | 148 | if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed 149 | Serial.print("Up held this hard: "); 150 | Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC); 151 | } 152 | if(ps2x.Button(PSB_PAD_RIGHT)){ 153 | Serial.print("Right held this hard: "); 154 | Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC); 155 | } 156 | if(ps2x.Button(PSB_PAD_LEFT)){ 157 | Serial.print("LEFT held this hard: "); 158 | Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC); 159 | } 160 | if(ps2x.Button(PSB_PAD_DOWN)){ 161 | Serial.print("DOWN held this hard: "); 162 | Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC); 163 | } 164 | 165 | vibrate = ps2x.Analog(PSAB_CROSS); //this will set the large motor vibrate speed based on how hard you press the blue (X) button 166 | if (ps2x.NewButtonState()) { //will be TRUE if any button changes state (on to off, or off to on) 167 | if(ps2x.Button(PSB_L3)) 168 | Serial.println("L3 pressed"); 169 | if(ps2x.Button(PSB_R3)) 170 | Serial.println("R3 pressed"); 171 | if(ps2x.Button(PSB_L2)) 172 | Serial.println("L2 pressed"); 173 | if(ps2x.Button(PSB_R2)) 174 | Serial.println("R2 pressed"); 175 | if(ps2x.Button(PSB_TRIANGLE)) 176 | Serial.println("Triangle pressed"); 177 | } 178 | 179 | if(ps2x.ButtonPressed(PSB_CIRCLE)) //will be TRUE if button was JUST pressed 180 | Serial.println("Circle just pressed"); 181 | if(ps2x.NewButtonState(PSB_CROSS)) //will be TRUE if button was JUST pressed OR released 182 | Serial.println("X just changed"); 183 | if(ps2x.ButtonReleased(PSB_SQUARE)) //will be TRUE if button was JUST released 184 | Serial.println("Square just released"); 185 | 186 | if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE 187 | Serial.print("Stick Values:"); 188 | Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX 189 | Serial.print(","); 190 | Serial.print(ps2x.Analog(PSS_LX), DEC); 191 | Serial.print(","); 192 | Serial.print(ps2x.Analog(PSS_RY), DEC); 193 | Serial.print(","); 194 | Serial.println(ps2x.Analog(PSS_RX), DEC); 195 | } 196 | } 197 | delay(50); 198 | } 199 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2X_Example/PS2X_lib.h: -------------------------------------------------------------------------------- 1 | /****************************************************************** 2 | * Super amazing PS2 controller Arduino Library v1.8 3 | * details and example sketch: 4 | * http://www.billporter.info/?p=240 5 | * 6 | * Original code by Shutter on Arduino Forums 7 | * 8 | * Revamped, made into lib by and supporting continued development: 9 | * Bill Porter 10 | * www.billporter.info 11 | * 12 | * Contributers: 13 | * Eric Wetzel (thewetzel@gmail.com) 14 | * Kurt Eckhardt 15 | * 16 | * Lib version history 17 | * 0.1 made into library, added analog stick support. 18 | * 0.2 fixed config_gamepad miss-spelling 19 | * added new functions: 20 | * NewButtonState(); 21 | * NewButtonState(unsigned int); 22 | * ButtonPressed(unsigned int); 23 | * ButtonReleased(unsigned int); 24 | * removed 'PS' from beginning of ever function 25 | * 1.0 found and fixed bug that wasn't configuring controller 26 | * added ability to define pins 27 | * added time checking to reconfigure controller if not polled enough 28 | * Analog sticks and pressures all through 'ps2x.Analog()' function 29 | * added: 30 | * enableRumble(); 31 | * enablePressures(); 32 | * 1.1 33 | * added some debug stuff for end user. Reports if no controller found 34 | * added auto-increasing sentence delay to see if it helps compatibility. 35 | * 1.2 36 | * found bad math by Shutter for original clock. Was running at 50kHz, not the required 500kHz. 37 | * fixed some of the debug reporting. 38 | * 1.3 39 | * Changed clock back to 50kHz. CuriousInventor says it's suppose to be 500kHz, but doesn't seem to work for everybody. 40 | * 1.4 41 | * Removed redundant functions. 42 | * Fixed mode check to include two other possible modes the controller could be in. 43 | * Added debug code enabled by compiler directives. See below to enable debug mode. 44 | * Added button definitions for shapes as well as colors. 45 | * 1.41 46 | * Some simple bug fixes 47 | * Added Keywords.txt file 48 | * 1.5 49 | * Added proper Guitar Hero compatibility 50 | * Fixed issue with DEBUG mode, had to send serial at once instead of in bits 51 | * 1.6 52 | * Changed config_gamepad() call to include rumble and pressures options 53 | * This was to fix controllers that will only go into config mode once 54 | * Old methods should still work for backwards compatibility 55 | * 1.7 56 | * Integrated Kurt's fixes for the interrupts messing with servo signals 57 | * Reorganized directory so examples show up in Arduino IDE menu 58 | * 1.8 59 | * Added Arduino 1.0 compatibility. 60 | * 1.9 61 | * Kurt - Added detection and recovery from dropping from analog mode, plus 62 | * integrated Chipkit (pic32mx...) support 63 | * 64 | * 65 | * 66 | *This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or(at your option) any later version. 67 | This program is distributed in the hope that it will be useful, 68 | but WITHOUT ANY WARRANTY; without even the implied warranty of 69 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 70 | GNU General Public License for more details. 71 | 72 | * 73 | ******************************************************************/ 74 | 75 | // $$$$$$$$$$$$ DEBUG ENABLE SECTION $$$$$$$$$$$$$$$$ 76 | // to debug ps2 controller, uncomment these two lines to print out debug to uart 77 | #define PS2X_DEBUG 78 | #define PS2X_COM_DEBUG 79 | 80 | #ifndef PS2X_lib_h 81 | #define PS2X_lib_h 82 | 83 | #if ARDUINO > 22 84 | #include "Arduino.h" 85 | #else 86 | #include "WProgram.h" 87 | #endif 88 | 89 | #include 90 | #include 91 | #include 92 | #ifdef __AVR__ 93 | // AVR 94 | #include 95 | #define CTRL_CLK 4 96 | #define CTRL_BYTE_DELAY 3 97 | #else 98 | #ifdef ESP8266 || ESP32 99 | #define CTRL_CLK 5 100 | #define CTRL_CLK_HIGH 5 101 | #define CTRL_BYTE_DELAY 18 102 | #else 103 | // Pic32... 104 | #include 105 | #define CTRL_CLK 5 106 | #define CTRL_CLK_HIGH 5 107 | #define CTRL_BYTE_DELAY 4 108 | #endif 109 | #endif 110 | 111 | //These are our button constants 112 | #define PSB_SELECT 0x0001 113 | #define PSB_L3 0x0002 114 | #define PSB_R3 0x0004 115 | #define PSB_START 0x0008 116 | #define PSB_PAD_UP 0x0010 117 | #define PSB_PAD_RIGHT 0x0020 118 | #define PSB_PAD_DOWN 0x0040 119 | #define PSB_PAD_LEFT 0x0080 120 | #define PSB_L2 0x0100 121 | #define PSB_R2 0x0200 122 | #define PSB_L1 0x0400 123 | #define PSB_R1 0x0800 124 | #define PSB_GREEN 0x1000 125 | #define PSB_RED 0x2000 126 | #define PSB_BLUE 0x4000 127 | #define PSB_PINK 0x8000 128 | #define PSB_TRIANGLE 0x1000 129 | #define PSB_CIRCLE 0x2000 130 | #define PSB_CROSS 0x4000 131 | #define PSB_SQUARE 0x8000 132 | 133 | //Guitar button constants 134 | #define UP_STRUM 0x0010 135 | #define DOWN_STRUM 0x0040 136 | #define LEFT_STRUM 0x0080 137 | #define RIGHT_STRUM 0x0020 138 | #define STAR_POWER 0x0100 139 | #define GREEN_FRET 0x0200 140 | #define YELLOW_FRET 0x1000 141 | #define RED_FRET 0x2000 142 | #define BLUE_FRET 0x4000 143 | #define ORANGE_FRET 0x8000 144 | #define WHAMMY_BAR 8 145 | 146 | //These are stick values 147 | #define PSS_RX 5 148 | #define PSS_RY 6 149 | #define PSS_LX 7 150 | #define PSS_LY 8 151 | 152 | //These are analog buttons 153 | #define PSAB_PAD_RIGHT 9 154 | #define PSAB_PAD_UP 11 155 | #define PSAB_PAD_DOWN 12 156 | #define PSAB_PAD_LEFT 10 157 | #define PSAB_L2 19 158 | #define PSAB_R2 20 159 | #define PSAB_L1 17 160 | #define PSAB_R1 18 161 | #define PSAB_GREEN 13 162 | #define PSAB_RED 14 163 | #define PSAB_BLUE 15 164 | #define PSAB_PINK 16 165 | #define PSAB_TRIANGLE 13 166 | #define PSAB_CIRCLE 14 167 | #define PSAB_CROSS 15 168 | #define PSAB_SQUARE 16 169 | 170 | #define SET(x,y) (x|=(1< 5 | #include 6 | 7 | 8 | //**************************** pins ******************************************// 9 | #define SS_PIN SS//2//SS 10 | #define MISO_PIN MISO//19//MISO 11 | #define MISO_PIN MOSI//23//MOSI 12 | 13 | /*---------------------------[CC11XX - R/W and burst bits]---------------------------*/ 14 | #define WRITE_SINGLE_BYTE 0x00 15 | #define WRITE_BURST 0x40 16 | #define READ_SINGLE_BYTE 0x80 17 | #define READ_BURST 0xC0 18 | 19 | /*----------------------[CC11XX - config register]----------------------------*/ 20 | #define IOCFG3 0x00 // GDO3 output pin config 21 | #define IOCFG2 0x01 // GDO2 output pin config 22 | #define IOCFG1 0x02 // GDO1 output pin config 23 | #define IOCFG0 0x03 // GDO0 output pin config 24 | #define SYNC3 0x04 // Sync word configuration 25 | #define SYNC2 0x05 26 | #define SYNC1 0x06 27 | #define SYNC0 0x07 28 | #define SYNC_CFG1 0x08 // Sync word detection 29 | #define SYNC_CFG0 0x09 30 | #define DEVIATION_M 0x0A // Frequency Deviation 31 | #define MODCFG_DEV_E 0x0B // Modulation format and frequency deviation 32 | #define DCFILT_CFG 0x0C // Digital DC Removal 33 | #define PREAMBLE_CFG1 0x0D // Preamble length config 34 | #define PREAMBLE_CFG0 0x0E 35 | #define FREQ_IF_CFG 0x0F // RX Mixer frequency 36 | #define IQIC 0x10 // Digital image channel compensation 37 | #define CHAN_BW 0x11 // Frequency control word, low byte 38 | #define MDMCFG1 0x12 // Modem configuration 39 | #define MDMCFG0 0x13 // Modem configuration 40 | #define SYMBOL_RATE2 0x14 // Modem configuration 41 | #define SYMBOL_RATE1 0x15 // Modem configuration 42 | #define SYMBOL_RATE0 0x16 // Modem configuration 43 | #define AGC_REF 0x17 // Modem deviation setting 44 | #define AGC_CS_THR 0x18 // Main Radio Cntrl State Machine config 45 | #define AGC_GAIN_ADJUST 0x19 // Main Radio Cntrl State Machine config 46 | #define AGC_CFG3 0x1A // Main Radio Cntrl State Machine config 47 | #define AGC_CFG2 0x1B // Frequency Offset Compensation config 48 | #define AGC_CFG1 0x1C // Bit Synchronization configuration 49 | #define AGC_CFG0 0x1D // AGC control 50 | #define FIFO_CFG 0x1E // AGC control 51 | #define DEV_ADDR 0x1F // AGC control 52 | #define SETTLING_CFG 0x20 // High byte Event 0 timeout 53 | #define FS_CFG 0x21 // Low byte Event 0 timeout 54 | #define WOR_CFG1 0x22 // Wake On Radio control 55 | #define WOR_CFG0 0x23 // Front end RX configuration 56 | #define WOR_EVENT0_MSB 0x24 // Front end TX configuration 57 | #define WOR_EVENT0_LSB 0x25 // Frequency synthesizer calibration 58 | #define PKT_CFG2 0x26 // Frequency synthesizer calibration 59 | #define PKT_CFG1 0x27 // Frequency synthesizer calibration 60 | #define PKT_CFG0 0x28 // Frequency synthesizer calibration 61 | #define RFEND_CFG1 0x29 // RC oscillator configuration 62 | #define RFEND_CFG0 0x2A // RC oscillator configuration 63 | #define PA_CFG2 0x2B // Frequency synthesizer cal control 64 | #define PA_CFG1 0x2C // Production test 65 | #define PA_CFG0 0x2D // AGC test 66 | #define PKT_LEN 0x2E // Various test settings 67 | 68 | /*----------------------[CC11XX - misc]---------------------------------------*/ 69 | #define EXTD_REGISTER 0x2F //Extended register space 70 | 71 | /*------------------------[CC11XX-command strobes]----------------------------*/ 72 | #define SRES 0x30 // Reset chip 73 | #define SFSTXON 0x31 // Enable/calibrate freq synthesizer 74 | #define SXOFF 0x32 // Turn off crystal oscillator. 75 | #define SCAL 0x33 // Calibrate freq synthesizer & disable 76 | #define SRX 0x34 // Enable RX. 77 | #define STX 0x35 // Enable TX. 78 | #define SIDLE 0x36 // Exit RX / TX 79 | #define SAFC 0x37 // AFC adjustment of freq synthesizer 80 | #define SWOR 0x38 // Start automatic RX polling sequence 81 | #define SPWD 0x39 // Enter pwr down mode when CSn goes hi 82 | #define SFRX 0x3A // Flush the RX FIFO buffer. 83 | #define SFTX 0x3B // Flush the TX FIFO buffer. 84 | #define SWORRST 0x3C // Reset real time clock. 85 | #define SNOP 0x3D // No operation. 86 | 87 | /*----------------------[CC11XX - extended registers]----------------------------*/ 88 | #define IF_MIX_CFG 0x00 89 | #define TOC_CFG 0x02 90 | #define FREQ2 0x0C 91 | #define FREQ1 0x0D 92 | #define FREQ0 0x0E 93 | #define IF_ADC0 0x11 94 | #define FS_DIG1 0x12 95 | #define FS_DIG0 0x13 96 | #define FS_CAL2 0x15 // used in manual calibration 97 | #define FS_CAL0 0x17 98 | #define FS_CHP 0x18 // used in manual calibration 99 | #define FS_DIVTWO 0x19 100 | #define FS_DSM0 0x1B 101 | #define FS_DVC0 0x1D 102 | #define FS_PFD 0x1F 103 | #define FS_PRE 0x20 104 | #define FS_REG_DIV_CML 0x21 105 | #define FS_SPARE 0x22 106 | #define FS_VCO4 0x23 // used in manual calibration 107 | #define FS_VCO2 0x25 // used in manual calibration 108 | #define XOSC5 0x32 109 | #define XOSC3 0x34 110 | #define XOSC1 0x36 111 | #define MARCSTATE 0x73 // Control state machine state 112 | #define PARTNUM 0x8F // Part number 113 | #define VERSION 0x90 // Current version number 114 | #define NUM_RXBYTES 0xD7 115 | #define FIFO_NUM_RX 0xD9 // Number of available bytes in RXFIFO 116 | 117 | /*------------------------[CC11XX - FIFO commands]----------------------------*/ 118 | #define STANDARD_FIFO 0x3F //write single only 119 | 120 | class CC1125 121 | { 122 | private: 123 | //void commandStrobe(uint8_t instr) {STATUS = spiRead(instr);}; 124 | 125 | uint8_t STATUS; 126 | //SPISettings s = SPISettings(4000000, MSBFIRST, SPI_MODE0); 127 | SPISettings s = SPISettings(8000000, MSBFIRST, SPI_MODE0); 128 | 129 | public: 130 | void commandStrobe(uint8_t instr); 131 | //uninitalised pointers to SPI objects 132 | SPIClass * vspi = NULL; 133 | bool begin(void); 134 | void set_idle(void); 135 | void receive(void); 136 | void transmit(void); 137 | void tx_payload(uint8_t *txbuffer, uint8_t len); 138 | void sendPacket(uint8_t *txbuffer, uint8_t len); 139 | bool get_packet(uint8_t rxbuffer[], uint8_t &pktlen); 140 | void manualCalibration(void); 141 | void spiWrite(uint8_t reg, uint8_t data, uint8_t prefix=0); 142 | uint8_t spiRead(uint8_t reg, uint8_t prefix=0); 143 | }; 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_CC1125_Receive/PS2_CC1125_Receive.ino: -------------------------------------------------------------------------------- 1 | #include "CC1125.h" 2 | 3 | // HW Pins 4 | #define PWMA 32 5 | #define PWMB 33 6 | #define PWMC 14 7 | #define PWMD 12 8 | #define VBAT_ADC 27 9 | #define IMU_INTERRUPT 25 10 | #define RF_INTERRUPT 15 11 | #define LED_PIN 0 12 | 13 | // setting PWM properties 14 | #define FREQ 60000 15 | #define RESOLUTION 10 16 | #define ledChannelA 0 17 | #define ledChannelB 1 18 | #define ledChannelC 2 19 | #define ledChannelD 3 20 | 21 | #define MAX_PWM (1< 5 | #include 6 | 7 | 8 | //**************************** pins ******************************************// 9 | #define SS_PIN SS//2//SS 10 | #define MISO_PIN MISO//19//MISO 11 | #define MISO_PIN MOSI//23//MOSI 12 | 13 | /*---------------------------[CC11XX - R/W and burst bits]---------------------------*/ 14 | #define WRITE_SINGLE_BYTE 0x00 15 | #define WRITE_BURST 0x40 16 | #define READ_SINGLE_BYTE 0x80 17 | #define READ_BURST 0xC0 18 | 19 | /*----------------------[CC11XX - config register]----------------------------*/ 20 | #define IOCFG3 0x00 // GDO3 output pin config 21 | #define IOCFG2 0x01 // GDO2 output pin config 22 | #define IOCFG1 0x02 // GDO1 output pin config 23 | #define IOCFG0 0x03 // GDO0 output pin config 24 | #define SYNC3 0x04 // Sync word configuration 25 | #define SYNC2 0x05 26 | #define SYNC1 0x06 27 | #define SYNC0 0x07 28 | #define SYNC_CFG1 0x08 // Sync word detection 29 | #define SYNC_CFG0 0x09 30 | #define DEVIATION_M 0x0A // Frequency Deviation 31 | #define MODCFG_DEV_E 0x0B // Modulation format and frequency deviation 32 | #define DCFILT_CFG 0x0C // Digital DC Removal 33 | #define PREAMBLE_CFG1 0x0D // Preamble length config 34 | #define PREAMBLE_CFG0 0x0E 35 | #define FREQ_IF_CFG 0x0F // RX Mixer frequency 36 | #define IQIC 0x10 // Digital image channel compensation 37 | #define CHAN_BW 0x11 // Frequency control word, low byte 38 | #define MDMCFG1 0x12 // Modem configuration 39 | #define MDMCFG0 0x13 // Modem configuration 40 | #define SYMBOL_RATE2 0x14 // Modem configuration 41 | #define SYMBOL_RATE1 0x15 // Modem configuration 42 | #define SYMBOL_RATE0 0x16 // Modem configuration 43 | #define AGC_REF 0x17 // Modem deviation setting 44 | #define AGC_CS_THR 0x18 // Main Radio Cntrl State Machine config 45 | #define AGC_GAIN_ADJUST 0x19 // Main Radio Cntrl State Machine config 46 | #define AGC_CFG3 0x1A // Main Radio Cntrl State Machine config 47 | #define AGC_CFG2 0x1B // Frequency Offset Compensation config 48 | #define AGC_CFG1 0x1C // Bit Synchronization configuration 49 | #define AGC_CFG0 0x1D // AGC control 50 | #define FIFO_CFG 0x1E // AGC control 51 | #define DEV_ADDR 0x1F // AGC control 52 | #define SETTLING_CFG 0x20 // High byte Event 0 timeout 53 | #define FS_CFG 0x21 // Low byte Event 0 timeout 54 | #define WOR_CFG1 0x22 // Wake On Radio control 55 | #define WOR_CFG0 0x23 // Front end RX configuration 56 | #define WOR_EVENT0_MSB 0x24 // Front end TX configuration 57 | #define WOR_EVENT0_LSB 0x25 // Frequency synthesizer calibration 58 | #define PKT_CFG2 0x26 // Frequency synthesizer calibration 59 | #define PKT_CFG1 0x27 // Frequency synthesizer calibration 60 | #define PKT_CFG0 0x28 // Frequency synthesizer calibration 61 | #define RFEND_CFG1 0x29 // RC oscillator configuration 62 | #define RFEND_CFG0 0x2A // RC oscillator configuration 63 | #define PA_CFG2 0x2B // Frequency synthesizer cal control 64 | #define PA_CFG1 0x2C // Production test 65 | #define PA_CFG0 0x2D // AGC test 66 | #define PKT_LEN 0x2E // Various test settings 67 | 68 | /*----------------------[CC11XX - misc]---------------------------------------*/ 69 | #define EXTD_REGISTER 0x2F //Extended register space 70 | 71 | /*------------------------[CC11XX-command strobes]----------------------------*/ 72 | #define SRES 0x30 // Reset chip 73 | #define SFSTXON 0x31 // Enable/calibrate freq synthesizer 74 | #define SXOFF 0x32 // Turn off crystal oscillator. 75 | #define SCAL 0x33 // Calibrate freq synthesizer & disable 76 | #define SRX 0x34 // Enable RX. 77 | #define STX 0x35 // Enable TX. 78 | #define SIDLE 0x36 // Exit RX / TX 79 | #define SAFC 0x37 // AFC adjustment of freq synthesizer 80 | #define SWOR 0x38 // Start automatic RX polling sequence 81 | #define SPWD 0x39 // Enter pwr down mode when CSn goes hi 82 | #define SFRX 0x3A // Flush the RX FIFO buffer. 83 | #define SFTX 0x3B // Flush the TX FIFO buffer. 84 | #define SWORRST 0x3C // Reset real time clock. 85 | #define SNOP 0x3D // No operation. 86 | 87 | /*----------------------[CC11XX - extended registers]----------------------------*/ 88 | #define IF_MIX_CFG 0x00 89 | #define TOC_CFG 0x02 90 | #define FREQ2 0x0C 91 | #define FREQ1 0x0D 92 | #define FREQ0 0x0E 93 | #define IF_ADC0 0x11 94 | #define FS_DIG1 0x12 95 | #define FS_DIG0 0x13 96 | #define FS_CAL2 0x15 // used in manual calibration 97 | #define FS_CAL0 0x17 98 | #define FS_CHP 0x18 // used in manual calibration 99 | #define FS_DIVTWO 0x19 100 | #define FS_DSM0 0x1B 101 | #define FS_DVC0 0x1D 102 | #define FS_PFD 0x1F 103 | #define FS_PRE 0x20 104 | #define FS_REG_DIV_CML 0x21 105 | #define FS_SPARE 0x22 106 | #define FS_VCO4 0x23 // used in manual calibration 107 | #define FS_VCO2 0x25 // used in manual calibration 108 | #define XOSC5 0x32 109 | #define XOSC3 0x34 110 | #define XOSC1 0x36 111 | #define MARCSTATE 0x73 // Control state machine state 112 | #define PARTNUM 0x8F // Part number 113 | #define VERSION 0x90 // Current version number 114 | #define NUM_RXBYTES 0xD7 115 | #define FIFO_NUM_RX 0xD9 // Number of available bytes in RXFIFO 116 | 117 | /*------------------------[CC11XX - FIFO commands]----------------------------*/ 118 | #define STANDARD_FIFO 0x3F //write single only 119 | 120 | class CC1125 121 | { 122 | private: 123 | //void commandStrobe(uint8_t instr) {STATUS = spiRead(instr);}; 124 | 125 | uint8_t STATUS; 126 | //SPISettings s = SPISettings(4000000, MSBFIRST, SPI_MODE0); 127 | SPISettings s = SPISettings(8000000, MSBFIRST, SPI_MODE0); 128 | 129 | public: 130 | void commandStrobe(uint8_t instr); 131 | //uninitalised pointers to SPI objects 132 | SPIClass * vspi = NULL; 133 | bool begin(void); 134 | void set_idle(void); 135 | void receive(void); 136 | void transmit(void); 137 | void tx_payload(uint8_t *txbuffer, uint8_t len); 138 | void sendPacket(uint8_t *txbuffer, uint8_t len); 139 | bool get_packet(uint8_t rxbuffer[], uint8_t &pktlen); 140 | void manualCalibration(void); 141 | void spiWrite(uint8_t reg, uint8_t data, uint8_t prefix=0); 142 | uint8_t spiRead(uint8_t reg, uint8_t prefix=0); 143 | }; 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_CC1125_Transmit/PS2_CC1125_Transmit.ino: -------------------------------------------------------------------------------- 1 | #include "CC1125.h" 2 | #include "PsxLib.h" 3 | 4 | CC1125 rf_comm; 5 | Psx ps2x; 6 | 7 | // Message buffers 8 | uint8_t txBuffer[128] = {0}; 9 | uint8_t pkt_size = 0; 10 | 11 | int joystick_map(int value, float low_input, float middle_input, float high_input, float low_output, float high_output) 12 | { 13 | float middle_output = (low_output + high_output)/2.0; 14 | if(value <= middle_input) 15 | { 16 | return map(value, low_input, middle_input, low_output, middle_output); 17 | } 18 | else 19 | { 20 | return map(value, middle_input, high_input, middle_output, high_output); 21 | } 22 | } 23 | 24 | void setup() { 25 | // put your setup code here, to run once: 26 | Serial.begin(115200); 27 | //Serial.begin(500000); 28 | delay(1000); 29 | Serial.println(""); 30 | Serial.println("Starting"); 31 | 32 | rf_comm.begin(); 33 | Serial.println("CC1125 Initialized!"); 34 | rf_comm.manualCalibration(); 35 | Serial.println("Finished calibration"); 36 | 37 | ps2x.setupPins(26, 27, 25, 32, 50); // dataPin, cmndPin, attPin, clockPin, delay 38 | Serial.println("PS2 Controller Initialized"); 39 | } 40 | 41 | void loop() { 42 | 43 | ps2x.read(); 44 | 45 | uint8_t len = 16; 46 | txBuffer[0] = len -1; 47 | txBuffer[1] = 0xAA; // Receiver address 48 | 49 | // Joystick values 50 | txBuffer[2] = joystick_map(ps2x.analog(0), 0, 123, 255, 0, 255); // horizontal right 51 | txBuffer[3] = joystick_map(ps2x.analog(1), 0, 123, 255, 255, 0); // vertical right 52 | txBuffer[4] = joystick_map(ps2x.analog(2), 0, 123, 255, 0, 255); // horizontal left 53 | txBuffer[5] = joystick_map(ps2x.analog(3), 0, 123, 255, 255, 0); // vertical left 54 | 55 | txBuffer[6] = ps2x.button(PSB_START); 56 | txBuffer[7] = ps2x.button(PSB_SELECT); 57 | 58 | txBuffer[8] = ps2x.button(PSB_TRIANGLE); 59 | txBuffer[9] = ps2x.button(PSB_CIRCLE); 60 | txBuffer[10] = ps2x.button(PSB_CROSS); 61 | txBuffer[11] = ps2x.button(PSB_SQUARE); 62 | 63 | txBuffer[12] = ps2x.button(PSB_L3); // Left stick press 64 | txBuffer[13] = ps2x.button(PSB_R3); // Right stick press 65 | 66 | txBuffer[14] = ps2x.button(PSB_L1); // Left back button 67 | txBuffer[15] = ps2x.button(PSB_R1); // Right back button 68 | 69 | rf_comm.sendPacket(txBuffer, len); 70 | delay(40); 71 | } 72 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_CC1125_Transmit/PsxLib.cpp: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Simple & Compact Library (PsxLib.cpp) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Psx side connected with two R10K with Clock and Data Pins. 19 | 20 | This program is free software: you can redistribute it and/or modify 21 | it under the terms of the GNU General Public License as published by 22 | the Free Software Foundation, either version 3 of the License, or 23 | (at your option) any later version. 24 | 25 | This program is distributed in the hope that it will be useful, 26 | but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | GNU General Public License for more details. 29 | 30 | You should have received a copy of the GNU General Public License 31 | along with this program. If not, see . 32 | */ 33 | #include "PsxLib.h" 34 | 35 | static byte enter_config[]={0x01,0x43,0x00,0x01,0x00}; 36 | static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00}; 37 | static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00}; 38 | static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A}; 39 | static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01}; 40 | static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A}; 41 | 42 | Psx::Psx() { }; // Class Psx 43 | 44 | byte Psx::shift(byte _dataOut) { // Does the actual shifting, both in and out simultaneously 45 | _dataIn = 0; 46 | 47 | for (byte i = 0; i < 8; i++) { 48 | 49 | if ( _dataOut & (1 << i) ) 50 | digitalWrite(_cmndPin, HIGH); // Writes out the _dataOut bits 51 | else 52 | digitalWrite(_cmndPin, LOW); 53 | 54 | digitalWrite(_clockPin, LOW); 55 | delayMicroseconds(_delay); 56 | 57 | if ( digitalRead(_dataPin) ) { // Reads the data pin 58 | _dataIn |= _BV(i); // Giuseppe Shift the read into _dataIn (align to the others libs) 59 | } 60 | 61 | digitalWrite(_clockPin, HIGH); 62 | delayMicroseconds(_delay); 63 | } 64 | return _dataIn; 65 | } 66 | 67 | 68 | void Psx::setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay) { 69 | pinMode(dataPin, INPUT_PULLUP); 70 | //digitalWrite(dataPin, HIGH); // Turn on internal pull-up 71 | pinMode(attPin, OUTPUT); 72 | digitalWrite(_attPin, HIGH); 73 | pinMode(cmndPin, OUTPUT); 74 | pinMode(clockPin, OUTPUT); 75 | digitalWrite(_clockPin, HIGH); 76 | 77 | _dataPin = dataPin; 78 | _cmndPin = cmndPin; 79 | _attPin = attPin; 80 | _clockPin = clockPin; 81 | _delay = delay; 82 | 83 | delayMicroseconds(1000000); 84 | 85 | delayMicroseconds(1000000); 86 | 87 | 88 | 89 | do 90 | { 91 | sendCommandString(enter_config, sizeof(enter_config)); //start config run 92 | sendCommandString(set_mode, sizeof(set_mode)); 93 | sendCommandString(exit_config, sizeof(exit_config)); 94 | 95 | read(); 96 | Serial.println(_mode, HEX); // Shoujld be -x73 97 | } 98 | while(_mode & 0xf0 != 0x70); 99 | } 100 | 101 | 102 | uint32_t Psx::read() { 103 | digitalWrite(_attPin, LOW); 104 | 105 | shift(0x01); 106 | _mode = shift(0x42); 107 | shift(0xFF); 108 | 109 | _data1 = ~shift(0xFF); 110 | _data2 = ~shift(0xFF); 111 | byte _data3 = shift(0xFF); 112 | byte _data4 = shift(0xFF); 113 | byte _data5 = shift(0xFF); 114 | byte _data6 = shift(0xFF); 115 | 116 | digitalWrite(_attPin, HIGH); 117 | 118 | _last_buttons = _buttonsData; // Giuseppe: Save Old Value 119 | 120 | _buttonsData = (_data2 << 8) | _data1; 121 | _sticksData = (_data6 << 24) | (_data5 << 16) | (_data4 << 8) | _data3; 122 | 123 | return _buttonsData; 124 | } 125 | 126 | bool Psx::button(unsigned int button) { 127 | return ((_buttonsData & button) > 0); 128 | } 129 | 130 | bool Psx::newButtonState() { 131 | return ((_last_buttons ^ _buttonsData) > 0); 132 | } 133 | 134 | bool Psx::buttonNewState(unsigned int button) { 135 | return (((_last_buttons ^ _buttonsData) & button) > 0); 136 | } 137 | 138 | byte Psx::analog(byte button) { 139 | return (uint8_t) (_sticksData >> ((button)*8)); 140 | } 141 | 142 | void Psx::sendCommandString(byte string[], byte len) { 143 | 144 | digitalWrite(_attPin, LOW); 145 | 146 | for (int y=0; y < len; y++) 147 | { 148 | shift(string[y]); 149 | } 150 | digitalWrite(_attPin, HIGH); 151 | } 152 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_CC1125_Transmit/PsxLib.h: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Library (PsxLib.h) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Cable side connected with two R10K with Clock and Data Pins. 19 | 20 | Controller protocol implemented using Andrew J McCubbin's analysis. 21 | http://www.gamesx.com/controldata/psxcont/psxcont.htm 22 | 23 | Shift command is based on tutorial examples for ShiftIn and ShiftOut 24 | functions both written by Carlyn Maw and Tom Igoe 25 | http://www.arduino.cc/en/Tutorial/ShiftIn 26 | http://www.arduino.cc/en/Tutorial/ShiftOut 27 | 28 | This program is free software: you can redistribute it and/or modify 29 | it under the terms of the GNU General Public License as published by 30 | the Free Software Foundation, either version 3 of the License, or 31 | (at your option) any later version. 32 | 33 | This program is distributed in the hope that it will be useful, 34 | but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | GNU General Public License for more details. 37 | 38 | You should have received a copy of the GNU General Public License 39 | along with this program. If not, see . 40 | */ 41 | 42 | 43 | #ifndef PSXLIB_H 44 | #define PSXLIB_H 45 | 46 | #if ARDUINO > 22 47 | #include "Arduino.h" 48 | #else 49 | #include "WProgram.h" 50 | #endif 51 | 52 | 53 | //These are our button constants 54 | #define PSB_IDLE 0x0000 55 | #define PSB_SELECT 0x0001 56 | #define PSB_START 0x0008 57 | #define PSB_L3 0x0002 58 | #define PSB_R3 0x0004 59 | #define PSB_L2 0x0100 60 | #define PSB_R2 0x0200 61 | #define PSB_L1 0x0400 62 | #define PSB_R1 0x0800 63 | #define PSB_GREEN 0x1000 64 | #define PSB_RED 0x2000 65 | #define PSB_BLUE 0x4000 66 | #define PSB_PINK 0x8000 67 | 68 | // Left Keys Pad and Stiks 69 | #define PSB_PAD_UP 0x0010 70 | #define PSB_PAD_RIGHT 0x0020 71 | #define PSB_PAD_DOWN 0x0040 72 | #define PSB_PAD_LEFT 0x0080 73 | 74 | #define PSB_LSTIK_LEFT 0x0010 75 | #define PSB_LSTIK_DOWN 0x0020 76 | #define PSB_LSTIK_RIGHT 0x0040 77 | #define PSB_LSTIK_UP 0x0080 78 | 79 | // Right Keys Pad and Stiks 80 | #define PSB_TRIANGLE 0x1000 81 | #define PSB_CIRCLE 0x2000 82 | #define PSB_CROSS 0x4000 83 | #define PSB_SQUARE 0x8000 84 | 85 | #define PSB_RSTIK_LEFT 0x1000 86 | #define PSB_RSTIK_DOWN 0x2000 87 | #define PSB_RSTIK_RIGHT 0x4000 88 | #define PSB_RSTIK_UP 0x8000 89 | 90 | class Psx { 91 | public: 92 | Psx(); 93 | void setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay); 94 | // Data Pin #, CMND Pin #, ATT Pin #, CLK Pin #, Delay 95 | // Delay is how long the clock goes without changing state in Microseconds. 96 | // It can be lowered to increase response, but if it is too low it may cause 97 | // glitches and have some keys spill over with false-positives. 98 | // A regular PSX controller works fine at 50 uSeconds. 99 | 100 | uint32_t read(); // Returns the status of the button presses in an unsignd int. 101 | // The value returned corresponds to each key as defined above. 102 | bool newButtonState(); // Giuseppe: will be true if any button was pressed 103 | bool buttonNewState(unsigned int); // Giuseppe: will be true if button changed state 104 | bool button(unsigned int); // Giuseppe: will be TRUE if button is being pressed 105 | byte analog(byte); 106 | 107 | private: 108 | void sendCommandString(byte*, byte); 109 | byte shift(byte _dataOut); 110 | 111 | byte _dataPin; 112 | byte _cmndPin; 113 | byte _attPin; 114 | byte _clockPin; 115 | 116 | byte _delay; 117 | boolean _temp; 118 | byte _dataIn; 119 | 120 | byte _mode; 121 | byte _data1; 122 | byte _data2; 123 | uint32_t _sticksData; 124 | uint32_t _buttonsData; 125 | uint32_t _last_buttons; // Giuseppe: Last Button Pressed 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_ESPNOW_Receive/PS2_ESPNOW_Receive.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Structure example to receive data 5 | // Must match the sender structure 6 | typedef struct struct_message { 7 | uint8_t hr_stick; 8 | uint8_t vr_stick; 9 | uint8_t hl_stick; 10 | uint8_t vl_stick; 11 | uint8_t start; 12 | uint8_t select; 13 | uint8_t triangle; 14 | uint8_t circle; 15 | uint8_t cross; 16 | uint8_t square; 17 | uint8_t l_stick_button; 18 | uint8_t r_stick_button; 19 | uint8_t l_back_button; 20 | uint8_t r_back_button; 21 | } struct_message; 22 | 23 | // Create a struct_message called myData 24 | struct_message controller_data; 25 | 26 | // callback function that will be executed when data is received 27 | void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { 28 | memcpy(&controller_data, incomingData, sizeof(controller_data)); 29 | 30 | Serial.print(controller_data.hr_stick); 31 | Serial.print(" "); 32 | Serial.print(controller_data.vr_stick); 33 | Serial.print(" "); 34 | Serial.print(controller_data.hl_stick); 35 | Serial.print(" "); 36 | Serial.print(controller_data.vl_stick); 37 | Serial.print(" "); 38 | Serial.print(controller_data.start); 39 | Serial.print(" "); 40 | Serial.print(controller_data.l_back_button); 41 | Serial.print(" "); 42 | Serial.println(controller_data.r_back_button); 43 | /* 44 | Serial.print("Bytes received: "); 45 | Serial.println(len); 46 | Serial.print("Char: "); 47 | Serial.println(myData.a); 48 | Serial.print("Int: "); 49 | Serial.println(myData.b); 50 | Serial.print("Float: "); 51 | Serial.println(myData.c); 52 | Serial.print("String: "); 53 | Serial.println(myData.d); 54 | Serial.print("Bool: "); 55 | Serial.println(myData.e); 56 | Serial.println(); 57 | */ 58 | } 59 | 60 | void setup() { 61 | // Initialize Serial Monitor 62 | Serial.begin(115200); 63 | 64 | // Set device as a Wi-Fi Station 65 | WiFi.mode(WIFI_STA); 66 | 67 | // Init ESP-NOW 68 | if (esp_now_init() != ESP_OK) { 69 | Serial.println("Error initializing ESP-NOW"); 70 | return; 71 | } 72 | 73 | // Once ESPNow is successfully Init, we will register for recv CB to 74 | // get recv packer info 75 | esp_now_register_recv_cb(OnDataRecv); 76 | } 77 | 78 | void loop() { 79 | 80 | } 81 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFReceive/PS2_NRFReceive.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "nRF24L01.h" 3 | #include "RF24.h" 4 | 5 | #define INTERRUPT_PIN 15 6 | 7 | struct RF24Data { 8 | byte throttle; 9 | byte yaw; 10 | byte pitch; 11 | byte roll; 12 | byte AUX1; 13 | byte AUX2; 14 | byte switches; 15 | }; 16 | 17 | const uint64_t pipeIn = 0xE8E8F0F0E1LL; //Remember, SAME AS TRANSMITTER CODE 18 | 19 | RF24 radio(4, 5, 18, 19, 23); // CE, CSN, SCK, MISO, MOSI 20 | 21 | RF24Data MyData; 22 | unsigned long prev_time = 0; 23 | 24 | bool msg_flag = false; 25 | void msg_flag_isr() 26 | { 27 | msg_flag = true; 28 | } 29 | 30 | 31 | void setup() { 32 | //Serial.begin(115200); 33 | Serial.begin(1000000); 34 | 35 | radio.begin(); 36 | radio.maskIRQ(1,1,0); 37 | //radio.setDataRate(RF24_250KBPS); 38 | //radio.setAutoAck(false); // Ensure autoACK is enabled 39 | 40 | radio.openReadingPipe(0, pipeIn); 41 | //radio.setPALevel(RF24_PA_MIN); 42 | radio.startListening(); 43 | 44 | 45 | 46 | //Serial.println(radio.isChipConnected()); 47 | 48 | radio.printDetails(); 49 | Serial.println(radio.isChipConnected()); 50 | 51 | delay(1000); 52 | 53 | pinMode(INTERRUPT_PIN, INPUT); 54 | attachInterrupt(INTERRUPT_PIN, msg_flag_isr, FALLING); 55 | 56 | prev_time = millis(); 57 | } 58 | 59 | void loop() { 60 | if (radio.available()) 61 | //if(msg_flag) 62 | { 63 | unsigned long current_time = millis(); 64 | int dt = current_time - prev_time; 65 | prev_time = current_time; 66 | 67 | radio.read(&MyData, sizeof(RF24Data)); 68 | //Serial.print(dt); 69 | //Serial.print(" "); 70 | Serial.print(MyData.throttle); 71 | Serial.print(" "); 72 | Serial.print(MyData.yaw); 73 | Serial.print(" "); 74 | Serial.print(MyData.pitch); 75 | Serial.print(" "); 76 | Serial.print(MyData.roll); 77 | Serial.print(" "); 78 | Serial.print(MyData.AUX1); 79 | Serial.print(" "); 80 | Serial.println(MyData.AUX2); 81 | 82 | msg_flag = false; 83 | } 84 | 85 | } 86 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFReceive/RF24_config.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Copyright (C) 2011 J. Coliz 4 | 5 | This program is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU General Public License 7 | version 2 as published by the Free Software Foundation. 8 | */ 9 | 10 | /* spaniakos 11 | Added __ARDUINO_X86__ support 12 | */ 13 | 14 | #ifndef __RF24_CONFIG_H__ 15 | #define __RF24_CONFIG_H__ 16 | 17 | /*** USER DEFINES: ***/ 18 | //#define FAILURE_HANDLING 19 | //#define SERIAL_DEBUG 20 | //#define MINIMAL 21 | //#define SPI_UART // Requires library from https://github.com/TMRh20/Sketches/tree/master/SPI_UART 22 | //#define SOFTSPI // Requires library from https://github.com/greiman/DigitalIO 23 | 24 | /**********************/ 25 | #define rf24_max(a,b) (a>b?a:b) 26 | #define rf24_min(a,b) (a 64 | 65 | // RF modules support 10 Mhz SPI bus speed 66 | const uint32_t RF24_SPI_SPEED = 10000000; 67 | 68 | #if defined (ARDUINO) && !defined (__arm__) && !defined (__ARDUINO_X86__) 69 | #if defined SPI_UART 70 | #include 71 | #define _SPI uspi 72 | #elif defined SOFTSPI 73 | // change these pins to your liking 74 | // 75 | #ifndef SOFT_SPI_MISO_PIN 76 | #define SOFT_SPI_MISO_PIN 9 77 | #endif 78 | 79 | #ifndef SOFT_SPI_MOSI_PIN 80 | #define SOFT_SPI_MOSI_PIN 8 81 | #endif 82 | 83 | #ifndef SOFT_SPI_SCK_PIN 84 | #define SOFT_SPI_SCK_PIN 7 85 | #endif 86 | const uint8_t SPI_MODE = 0; 87 | #define _SPI spi 88 | 89 | #else 90 | #include 91 | #define _SPI SPI 92 | #endif 93 | #else 94 | // Define _BV for non-Arduino platforms and for Arduino DUE 95 | #include 96 | #include 97 | #include 98 | 99 | 100 | #if defined(__arm__) || defined (__ARDUINO_X86__) 101 | #if defined (__arm__) && defined (SPI_UART) 102 | #include 103 | #define _SPI uspi 104 | #else 105 | #include 106 | #define _SPI SPI 107 | #endif 108 | #elif !defined(__arm__) && !defined (__ARDUINO_X86__) 109 | extern HardwareSPI SPI; 110 | #endif 111 | 112 | #define _BV(x) (1<<(x)) 113 | 114 | #endif 115 | 116 | #ifdef SERIAL_DEBUG 117 | #define IF_SERIAL_DEBUG(x) ({x;}) 118 | #else 119 | #define IF_SERIAL_DEBUG(x) 120 | #if defined(RF24_TINY) 121 | #define printf_P(...) 122 | #endif 123 | #endif 124 | 125 | #if defined (__ARDUINO_X86__) 126 | #define printf_P printf 127 | #define _BV(bit) (1<<(bit)) 128 | #endif 129 | 130 | // Progmem is Arduino-specific 131 | // Arduino DUE is arm and does not include avr/pgmspace 132 | #if defined (ARDUINO_ARCH_ESP8266) 133 | 134 | #include 135 | #define PRIPSTR "%s" 136 | 137 | #elif defined(ARDUINO) && !defined(ESP_PLATFORM) && ! defined(__arm__) && !defined (__ARDUINO_X86__) || defined(XMEGA) 138 | #include 139 | #define PRIPSTR "%S" 140 | #else 141 | #if ! defined(ARDUINO) // This doesn't work on Arduino DUE 142 | typedef char const char; 143 | #else // Fill in pgm_read_byte that is used, but missing from DUE 144 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 145 | #endif 146 | 147 | 148 | typedef uint16_t prog_uint16_t; 149 | #define PSTR(x) (x) 150 | #define printf_P printf 151 | #define strlen_P strlen 152 | #define PROGMEM 153 | #define pgm_read_word(p) (*(p)) 154 | 155 | #define PRIPSTR "%s" 156 | 157 | #endif 158 | 159 | #endif 160 | 161 | 162 | 163 | #endif // __RF24_CONFIG_H__ 164 | 165 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFReceive/nRF24L01.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2007 Stefan Engelke 3 | Portions Copyright (C) 2011 Greg Copeland 4 | 5 | Permission is hereby granted, free of charge, to any person 6 | obtaining a copy of this software and associated documentation 7 | files (the "Software"), to deal in the Software without 8 | restriction, including without limitation the rights to use, copy, 9 | modify, merge, publish, distribute, sublicense, and/or sell copies 10 | of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be 14 | included in all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 21 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 23 | DEALINGS IN THE SOFTWARE. 24 | */ 25 | 26 | /* Memory Map */ 27 | #define NRF_CONFIG 0x00 28 | #define EN_AA 0x01 29 | #define EN_RXADDR 0x02 30 | #define SETUP_AW 0x03 31 | #define SETUP_RETR 0x04 32 | #define RF_CH 0x05 33 | #define RF_SETUP 0x06 34 | #define NRF_STATUS 0x07 35 | #define OBSERVE_TX 0x08 36 | #define CD 0x09 37 | #define RX_ADDR_P0 0x0A 38 | #define RX_ADDR_P1 0x0B 39 | #define RX_ADDR_P2 0x0C 40 | #define RX_ADDR_P3 0x0D 41 | #define RX_ADDR_P4 0x0E 42 | #define RX_ADDR_P5 0x0F 43 | #define TX_ADDR 0x10 44 | #define RX_PW_P0 0x11 45 | #define RX_PW_P1 0x12 46 | #define RX_PW_P2 0x13 47 | #define RX_PW_P3 0x14 48 | #define RX_PW_P4 0x15 49 | #define RX_PW_P5 0x16 50 | #define FIFO_STATUS 0x17 51 | #define DYNPD 0x1C 52 | #define FEATURE 0x1D 53 | 54 | /* Bit Mnemonics */ 55 | #define MASK_RX_DR 6 56 | #define MASK_TX_DS 5 57 | #define MASK_MAX_RT 4 58 | #define EN_CRC 3 59 | #define CRCO 2 60 | #define PWR_UP 1 61 | #define PRIM_RX 0 62 | #define ENAA_P5 5 63 | #define ENAA_P4 4 64 | #define ENAA_P3 3 65 | #define ENAA_P2 2 66 | #define ENAA_P1 1 67 | #define ENAA_P0 0 68 | #define ERX_P5 5 69 | #define ERX_P4 4 70 | #define ERX_P3 3 71 | #define ERX_P2 2 72 | #define ERX_P1 1 73 | #define ERX_P0 0 74 | #define AW 0 75 | #define ARD 4 76 | #define ARC 0 77 | #define PLL_LOCK 4 78 | #define RF_DR 3 79 | #define RF_PWR 6 80 | #define RX_DR 6 81 | #define TX_DS 5 82 | #define MAX_RT 4 83 | #define RX_P_NO 1 84 | #define TX_FULL 0 85 | #define PLOS_CNT 4 86 | #define ARC_CNT 0 87 | #define TX_REUSE 6 88 | #define FIFO_FULL 5 89 | #define TX_EMPTY 4 90 | #define RX_FULL 1 91 | #define RX_EMPTY 0 92 | #define DPL_P5 5 93 | #define DPL_P4 4 94 | #define DPL_P3 3 95 | #define DPL_P2 2 96 | #define DPL_P1 1 97 | #define DPL_P0 0 98 | #define EN_DPL 2 99 | #define EN_ACK_PAY 1 100 | #define EN_DYN_ACK 0 101 | 102 | /* Instruction Mnemonics */ 103 | #define R_REGISTER 0x00 104 | #define W_REGISTER 0x20 105 | #define REGISTER_MASK 0x1F 106 | #define ACTIVATE 0x50 107 | #define R_RX_PL_WID 0x60 108 | #define R_RX_PAYLOAD 0x61 109 | #define W_TX_PAYLOAD 0xA0 110 | #define W_ACK_PAYLOAD 0xA8 111 | #define FLUSH_TX 0xE1 112 | #define FLUSH_RX 0xE2 113 | #define REUSE_TX_PL 0xE3 114 | #define RF24_NOP 0xFF 115 | 116 | /* Non-P omissions */ 117 | #define LNA_HCURR 0 118 | 119 | /* P model memory Map */ 120 | #define RPD 0x09 121 | #define W_TX_PAYLOAD_NO_ACK 0xB0 122 | 123 | /* P model bit Mnemonics */ 124 | #define RF_DR_LOW 5 125 | #define RF_DR_HIGH 3 126 | #define RF_PWR_LOW 1 127 | #define RF_PWR_HIGH 2 128 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFReceive/printf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2011 J. Coliz 3 | 4 | This program is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU General Public License 6 | version 2 as published by the Free Software Foundation. 7 | */ 8 | /* Galileo support from spaniakos */ 9 | 10 | /** 11 | * @file printf.h 12 | * 13 | * Setup necessary to direct stdout to the Arduino Serial library, which 14 | * enables 'printf' 15 | */ 16 | 17 | #ifndef __PRINTF_H__ 18 | #define __PRINTF_H__ 19 | 20 | #if defined (ARDUINO) && !defined (__arm__) && !defined(__ARDUINO_X86__) 21 | 22 | int serial_putc( char c, FILE * ) 23 | { 24 | Serial.write( c ); 25 | 26 | return c; 27 | } 28 | 29 | void printf_begin(void) 30 | { 31 | fdevopen( &serial_putc, 0 ); 32 | } 33 | 34 | #elif defined (__arm__) 35 | 36 | void printf_begin(void){} 37 | 38 | #elif defined(__ARDUINO_X86__) 39 | int serial_putc( char c, FILE * ) 40 | { 41 | Serial.write( c ); 42 | 43 | return c; 44 | } 45 | 46 | void printf_begin(void) 47 | { 48 | //JESUS - For reddirect stdout to /dev/ttyGS0 (Serial Monitor port) 49 | stdout = freopen("/dev/ttyGS0","w",stdout); 50 | delay(500); 51 | printf("redirecting to Serial..."); 52 | 53 | //JESUS ----------------------------------------------------------- 54 | } 55 | #else 56 | #error This example is only for use on Arduino. 57 | #endif // ARDUINO 58 | 59 | #endif // __PRINTF_H__ 60 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/PS2_NRFTransmit.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "nRF24L01.h" 3 | #include "RF24.h" 4 | #include "PsxLib.h" 5 | 6 | #define MULTIWII 1 7 | 8 | RF24 radio(4, 5, 18, 19, 23); // CE, CSN, SCK, MISO, MOSI 9 | Psx ps2x; 10 | 11 | const uint64_t pipeOut = 0xE8E8F0F0E1LL; //IMPORTANT: The same as in the receiver!!! 12 | 13 | byte prev_arm = 0; 14 | byte prev_AUX2 = 0; 15 | 16 | struct MyData { 17 | byte throttle; 18 | byte yaw; 19 | byte pitch; 20 | byte roll; 21 | byte arm; // AUX1 22 | byte AUX2; 23 | }; 24 | 25 | MyData data; 26 | 27 | int joystick_map(int value, float low_input, float middle_input, float high_input, float low_output, float high_output) 28 | { 29 | float middle_output = (low_output + high_output)/2.0; 30 | if(value <= middle_input) 31 | { 32 | return map(value, low_input, middle_input, low_output, middle_output); 33 | } 34 | else 35 | { 36 | return map(value, middle_input, high_input, middle_output, high_output); 37 | } 38 | } 39 | 40 | void setup() { 41 | Serial.begin(115200); 42 | Serial.println(""); 43 | 44 | #if MULTIWII 45 | radio.begin(); 46 | radio.setAutoAck(false); 47 | radio.setDataRate(RF24_250KBPS); 48 | radio.openWritingPipe(pipeOut); 49 | #else 50 | radio.begin(); 51 | //radio.setDataRate(RF24_250KBPS); 52 | radio.openWritingPipe(pipeOut); 53 | //radio.setPALevel(RF24_PA_MIN); 54 | radio.stopListening(); 55 | #endif 56 | 57 | pinMode(5, OUTPUT); 58 | digitalWrite(5, LOW); 59 | 60 | data.arm = 0; 61 | data.AUX2 = 0; 62 | 63 | ps2x.setupPins(26, 27, 25, 32, 50); // dataPin, cmndPin, attPin, clockPin, delay 64 | 65 | if(radio.isChipConnected()) 66 | { 67 | Serial.println("Radio is connected!"); 68 | Serial.println("Starting transmission!"); 69 | digitalWrite(5, HIGH); 70 | } 71 | else 72 | { 73 | Serial.println("Radio not found!"); 74 | digitalWrite(5, LOW); 75 | while(1); 76 | } 77 | 78 | } 79 | 80 | void loop() { 81 | 82 | ps2x.read(); 83 | 84 | data.throttle = joystick_map(ps2x.analog(3), 0, 123, 255, 255, 0); // vertical left 85 | data.yaw = joystick_map(ps2x.analog(2), 0, 123, 255, 0, 255); // horizontal left 86 | data.pitch = joystick_map(ps2x.analog(1), 0, 123, 255, 255, 0); // vertical right 87 | data.roll = joystick_map(ps2x.analog(0), 0, 123, 255, 0, 255); // horizontal right 88 | 89 | uint8_t temp_arm = ps2x.button(PSB_R1); 90 | if( temp_arm == 1 && prev_arm == 0) 91 | { 92 | //Serial.println("Switching"); 93 | if(data.arm == 0 && data.throttle == 0) 94 | { 95 | data.arm = 1; 96 | } 97 | else 98 | { 99 | data.arm = 0; 100 | } 101 | } 102 | prev_arm = temp_arm; 103 | 104 | uint8_t temp_aux2 = ps2x.button(PSB_L1); 105 | if( temp_aux2 == 1 && prev_AUX2 == 0) 106 | { 107 | data.AUX2 = !data.AUX2; 108 | } 109 | prev_AUX2 = temp_aux2; 110 | 111 | /*Serial.print(data.throttle); 112 | Serial.print(" "); 113 | Serial.print(data.yaw); 114 | Serial.print(" "); 115 | Serial.print(data.pitch); 116 | Serial.print(" "); 117 | Serial.print(data.roll); 118 | Serial.print(" "); 119 | Serial.print(data.arm); 120 | Serial.print(" "); 121 | Serial.println(data.AUX2);*/ 122 | 123 | radio.write(&data, sizeof(MyData)); 124 | //delay(10); 125 | } 126 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/PsxLib.cpp: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Simple & Compact Library (PsxLib.cpp) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Psx side connected with two R10K with Clock and Data Pins. 19 | 20 | This program is free software: you can redistribute it and/or modify 21 | it under the terms of the GNU General Public License as published by 22 | the Free Software Foundation, either version 3 of the License, or 23 | (at your option) any later version. 24 | 25 | This program is distributed in the hope that it will be useful, 26 | but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | GNU General Public License for more details. 29 | 30 | You should have received a copy of the GNU General Public License 31 | along with this program. If not, see . 32 | */ 33 | #include "PsxLib.h" 34 | 35 | static byte enter_config[]={0x01,0x43,0x00,0x01,0x00}; 36 | static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00}; 37 | static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00}; 38 | static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A}; 39 | static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01}; 40 | static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A}; 41 | 42 | Psx::Psx() { }; // Class Psx 43 | 44 | byte Psx::shift(byte _dataOut) { // Does the actual shifting, both in and out simultaneously 45 | _dataIn = 0; 46 | 47 | for (byte i = 0; i < 8; i++) { 48 | 49 | if ( _dataOut & (1 << i) ) 50 | digitalWrite(_cmndPin, HIGH); // Writes out the _dataOut bits 51 | else 52 | digitalWrite(_cmndPin, LOW); 53 | 54 | digitalWrite(_clockPin, LOW); 55 | delayMicroseconds(_delay); 56 | 57 | if ( digitalRead(_dataPin) ) { // Reads the data pin 58 | _dataIn |= _BV(i); // Giuseppe Shift the read into _dataIn (align to the others libs) 59 | } 60 | 61 | digitalWrite(_clockPin, HIGH); 62 | delayMicroseconds(_delay); 63 | } 64 | return _dataIn; 65 | } 66 | 67 | 68 | void Psx::setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay) { 69 | pinMode(dataPin, INPUT_PULLUP); 70 | //digitalWrite(dataPin, HIGH); // Turn on internal pull-up 71 | pinMode(attPin, OUTPUT); 72 | digitalWrite(_attPin, HIGH); 73 | pinMode(cmndPin, OUTPUT); 74 | pinMode(clockPin, OUTPUT); 75 | digitalWrite(_clockPin, HIGH); 76 | 77 | _dataPin = dataPin; 78 | _cmndPin = cmndPin; 79 | _attPin = attPin; 80 | _clockPin = clockPin; 81 | _delay = delay; 82 | 83 | delayMicroseconds(1000000); 84 | 85 | delayMicroseconds(1000000); 86 | 87 | 88 | 89 | do 90 | { 91 | sendCommandString(enter_config, sizeof(enter_config)); //start config run 92 | sendCommandString(set_mode, sizeof(set_mode)); 93 | sendCommandString(exit_config, sizeof(exit_config)); 94 | 95 | read(); 96 | Serial.println(_mode, HEX); // Shoujld be -x73 97 | } 98 | while(_mode & 0xf0 != 0x70); 99 | } 100 | 101 | 102 | uint32_t Psx::read() { 103 | digitalWrite(_attPin, LOW); 104 | 105 | shift(0x01); 106 | _mode = shift(0x42); 107 | shift(0xFF); 108 | 109 | _data1 = ~shift(0xFF); 110 | _data2 = ~shift(0xFF); 111 | byte _data3 = shift(0xFF); 112 | byte _data4 = shift(0xFF); 113 | byte _data5 = shift(0xFF); 114 | byte _data6 = shift(0xFF); 115 | 116 | digitalWrite(_attPin, HIGH); 117 | 118 | _last_buttons = _buttonsData; // Giuseppe: Save Old Value 119 | 120 | _buttonsData = (_data2 << 8) | _data1; 121 | _sticksData = (_data6 << 24) | (_data5 << 16) | (_data4 << 8) | _data3; 122 | 123 | return _buttonsData; 124 | } 125 | 126 | bool Psx::button(unsigned int button) { 127 | return ((_buttonsData & button) > 0); 128 | } 129 | 130 | bool Psx::newButtonState() { 131 | return ((_last_buttons ^ _buttonsData) > 0); 132 | } 133 | 134 | bool Psx::buttonNewState(unsigned int button) { 135 | return (((_last_buttons ^ _buttonsData) & button) > 0); 136 | } 137 | 138 | byte Psx::analog(byte button) { 139 | return (uint8_t) (_sticksData >> ((button)*8)); 140 | } 141 | 142 | void Psx::sendCommandString(byte string[], byte len) { 143 | 144 | digitalWrite(_attPin, LOW); 145 | 146 | for (int y=0; y < len; y++) 147 | { 148 | shift(string[y]); 149 | } 150 | digitalWrite(_attPin, HIGH); 151 | } 152 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/PsxLib.h: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Library (PsxLib.h) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Cable side connected with two R10K with Clock and Data Pins. 19 | 20 | Controller protocol implemented using Andrew J McCubbin's analysis. 21 | http://www.gamesx.com/controldata/psxcont/psxcont.htm 22 | 23 | Shift command is based on tutorial examples for ShiftIn and ShiftOut 24 | functions both written by Carlyn Maw and Tom Igoe 25 | http://www.arduino.cc/en/Tutorial/ShiftIn 26 | http://www.arduino.cc/en/Tutorial/ShiftOut 27 | 28 | This program is free software: you can redistribute it and/or modify 29 | it under the terms of the GNU General Public License as published by 30 | the Free Software Foundation, either version 3 of the License, or 31 | (at your option) any later version. 32 | 33 | This program is distributed in the hope that it will be useful, 34 | but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | GNU General Public License for more details. 37 | 38 | You should have received a copy of the GNU General Public License 39 | along with this program. If not, see . 40 | */ 41 | 42 | 43 | #ifndef PSXLIB_H 44 | #define PSXLIB_H 45 | 46 | #if ARDUINO > 22 47 | #include "Arduino.h" 48 | #else 49 | #include "WProgram.h" 50 | #endif 51 | 52 | 53 | //These are our button constants 54 | #define PSB_IDLE 0x0000 55 | #define PSB_SELECT 0x0001 56 | #define PSB_START 0x0008 57 | #define PSB_L3 0x0002 58 | #define PSB_R3 0x0004 59 | #define PSB_L2 0x0100 60 | #define PSB_R2 0x0200 61 | #define PSB_L1 0x0400 62 | #define PSB_R1 0x0800 63 | #define PSB_GREEN 0x1000 64 | #define PSB_RED 0x2000 65 | #define PSB_BLUE 0x4000 66 | #define PSB_PINK 0x8000 67 | 68 | // Left Keys Pad and Stiks 69 | #define PSB_PAD_UP 0x0010 70 | #define PSB_PAD_RIGHT 0x0020 71 | #define PSB_PAD_DOWN 0x0040 72 | #define PSB_PAD_LEFT 0x0080 73 | 74 | #define PSB_LSTIK_LEFT 0x0010 75 | #define PSB_LSTIK_DOWN 0x0020 76 | #define PSB_LSTIK_RIGHT 0x0040 77 | #define PSB_LSTIK_UP 0x0080 78 | 79 | // Right Keys Pad and Stiks 80 | #define PSB_TRIANGLE 0x1000 81 | #define PSB_CIRCLE 0x2000 82 | #define PSB_CROSS 0x4000 83 | #define PSB_SQUARE 0x8000 84 | 85 | #define PSB_RSTIK_LEFT 0x1000 86 | #define PSB_RSTIK_DOWN 0x2000 87 | #define PSB_RSTIK_RIGHT 0x4000 88 | #define PSB_RSTIK_UP 0x8000 89 | 90 | class Psx { 91 | public: 92 | Psx(); 93 | void setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay); 94 | // Data Pin #, CMND Pin #, ATT Pin #, CLK Pin #, Delay 95 | // Delay is how long the clock goes without changing state in Microseconds. 96 | // It can be lowered to increase response, but if it is too low it may cause 97 | // glitches and have some keys spill over with false-positives. 98 | // A regular PSX controller works fine at 50 uSeconds. 99 | 100 | uint32_t read(); // Returns the status of the button presses in an unsignd int. 101 | // The value returned corresponds to each key as defined above. 102 | bool newButtonState(); // Giuseppe: will be true if any button was pressed 103 | bool buttonNewState(unsigned int); // Giuseppe: will be true if button changed state 104 | bool button(unsigned int); // Giuseppe: will be TRUE if button is being pressed 105 | byte analog(byte); 106 | 107 | private: 108 | void sendCommandString(byte*, byte); 109 | byte shift(byte _dataOut); 110 | 111 | byte _dataPin; 112 | byte _cmndPin; 113 | byte _attPin; 114 | byte _clockPin; 115 | 116 | byte _delay; 117 | boolean _temp; 118 | byte _dataIn; 119 | 120 | byte _mode; 121 | byte _data1; 122 | byte _data2; 123 | uint32_t _sticksData; 124 | uint32_t _buttonsData; 125 | uint32_t _last_buttons; // Giuseppe: Last Button Pressed 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/RF24_config.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Copyright (C) 2011 J. Coliz 4 | 5 | This program is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU General Public License 7 | version 2 as published by the Free Software Foundation. 8 | */ 9 | 10 | /* spaniakos 11 | Added __ARDUINO_X86__ support 12 | */ 13 | 14 | #ifndef __RF24_CONFIG_H__ 15 | #define __RF24_CONFIG_H__ 16 | 17 | /*** USER DEFINES: ***/ 18 | //#define FAILURE_HANDLING 19 | //#define SERIAL_DEBUG 20 | //#define MINIMAL 21 | //#define SPI_UART // Requires library from https://github.com/TMRh20/Sketches/tree/master/SPI_UART 22 | //#define SOFTSPI // Requires library from https://github.com/greiman/DigitalIO 23 | 24 | /**********************/ 25 | #define rf24_max(a,b) (a>b?a:b) 26 | #define rf24_min(a,b) (a 64 | 65 | // RF modules support 10 Mhz SPI bus speed 66 | const uint32_t RF24_SPI_SPEED = 10000000; 67 | 68 | #if defined (ARDUINO) && !defined (__arm__) && !defined (__ARDUINO_X86__) 69 | #if defined SPI_UART 70 | #include 71 | #define _SPI uspi 72 | #elif defined SOFTSPI 73 | // change these pins to your liking 74 | // 75 | #ifndef SOFT_SPI_MISO_PIN 76 | #define SOFT_SPI_MISO_PIN 9 77 | #endif 78 | 79 | #ifndef SOFT_SPI_MOSI_PIN 80 | #define SOFT_SPI_MOSI_PIN 8 81 | #endif 82 | 83 | #ifndef SOFT_SPI_SCK_PIN 84 | #define SOFT_SPI_SCK_PIN 7 85 | #endif 86 | const uint8_t SPI_MODE = 0; 87 | #define _SPI spi 88 | 89 | #else 90 | #include 91 | #define _SPI SPI 92 | #endif 93 | #else 94 | // Define _BV for non-Arduino platforms and for Arduino DUE 95 | #include 96 | #include 97 | #include 98 | 99 | 100 | #if defined(__arm__) || defined (__ARDUINO_X86__) 101 | #if defined (__arm__) && defined (SPI_UART) 102 | #include 103 | #define _SPI uspi 104 | #else 105 | #include 106 | #define _SPI SPI 107 | #endif 108 | #elif !defined(__arm__) && !defined (__ARDUINO_X86__) 109 | extern HardwareSPI SPI; 110 | #endif 111 | 112 | #define _BV(x) (1<<(x)) 113 | 114 | #endif 115 | 116 | #ifdef SERIAL_DEBUG 117 | #define IF_SERIAL_DEBUG(x) ({x;}) 118 | #else 119 | #define IF_SERIAL_DEBUG(x) 120 | #if defined(RF24_TINY) 121 | #define printf_P(...) 122 | #endif 123 | #endif 124 | 125 | #if defined (__ARDUINO_X86__) 126 | #define printf_P printf 127 | #define _BV(bit) (1<<(bit)) 128 | #endif 129 | 130 | // Progmem is Arduino-specific 131 | // Arduino DUE is arm and does not include avr/pgmspace 132 | #if defined (ARDUINO_ARCH_ESP8266) 133 | 134 | #include 135 | #define PRIPSTR "%s" 136 | 137 | #elif defined(ARDUINO) && !defined(ESP_PLATFORM) && ! defined(__arm__) && !defined (__ARDUINO_X86__) || defined(XMEGA) 138 | #include 139 | #define PRIPSTR "%S" 140 | #else 141 | #if ! defined(ARDUINO) // This doesn't work on Arduino DUE 142 | typedef char const char; 143 | #else // Fill in pgm_read_byte that is used, but missing from DUE 144 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 145 | #endif 146 | 147 | 148 | typedef uint16_t prog_uint16_t; 149 | #define PSTR(x) (x) 150 | #define printf_P printf 151 | #define strlen_P strlen 152 | #define PROGMEM 153 | #define pgm_read_word(p) (*(p)) 154 | 155 | #define PRIPSTR "%s" 156 | 157 | #endif 158 | 159 | #endif 160 | 161 | 162 | 163 | #endif // __RF24_CONFIG_H__ 164 | 165 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/nRF24L01.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2007 Stefan Engelke 3 | Portions Copyright (C) 2011 Greg Copeland 4 | 5 | Permission is hereby granted, free of charge, to any person 6 | obtaining a copy of this software and associated documentation 7 | files (the "Software"), to deal in the Software without 8 | restriction, including without limitation the rights to use, copy, 9 | modify, merge, publish, distribute, sublicense, and/or sell copies 10 | of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be 14 | included in all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 21 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 23 | DEALINGS IN THE SOFTWARE. 24 | */ 25 | 26 | /* Memory Map */ 27 | #define NRF_CONFIG 0x00 28 | #define EN_AA 0x01 29 | #define EN_RXADDR 0x02 30 | #define SETUP_AW 0x03 31 | #define SETUP_RETR 0x04 32 | #define RF_CH 0x05 33 | #define RF_SETUP 0x06 34 | #define NRF_STATUS 0x07 35 | #define OBSERVE_TX 0x08 36 | #define CD 0x09 37 | #define RX_ADDR_P0 0x0A 38 | #define RX_ADDR_P1 0x0B 39 | #define RX_ADDR_P2 0x0C 40 | #define RX_ADDR_P3 0x0D 41 | #define RX_ADDR_P4 0x0E 42 | #define RX_ADDR_P5 0x0F 43 | #define TX_ADDR 0x10 44 | #define RX_PW_P0 0x11 45 | #define RX_PW_P1 0x12 46 | #define RX_PW_P2 0x13 47 | #define RX_PW_P3 0x14 48 | #define RX_PW_P4 0x15 49 | #define RX_PW_P5 0x16 50 | #define FIFO_STATUS 0x17 51 | #define DYNPD 0x1C 52 | #define FEATURE 0x1D 53 | 54 | /* Bit Mnemonics */ 55 | #define MASK_RX_DR 6 56 | #define MASK_TX_DS 5 57 | #define MASK_MAX_RT 4 58 | #define EN_CRC 3 59 | #define CRCO 2 60 | #define PWR_UP 1 61 | #define PRIM_RX 0 62 | #define ENAA_P5 5 63 | #define ENAA_P4 4 64 | #define ENAA_P3 3 65 | #define ENAA_P2 2 66 | #define ENAA_P1 1 67 | #define ENAA_P0 0 68 | #define ERX_P5 5 69 | #define ERX_P4 4 70 | #define ERX_P3 3 71 | #define ERX_P2 2 72 | #define ERX_P1 1 73 | #define ERX_P0 0 74 | #define AW 0 75 | #define ARD 4 76 | #define ARC 0 77 | #define PLL_LOCK 4 78 | #define RF_DR 3 79 | #define RF_PWR 6 80 | #define RX_DR 6 81 | #define TX_DS 5 82 | #define MAX_RT 4 83 | #define RX_P_NO 1 84 | #define TX_FULL 0 85 | #define PLOS_CNT 4 86 | #define ARC_CNT 0 87 | #define TX_REUSE 6 88 | #define FIFO_FULL 5 89 | #define TX_EMPTY 4 90 | #define RX_FULL 1 91 | #define RX_EMPTY 0 92 | #define DPL_P5 5 93 | #define DPL_P4 4 94 | #define DPL_P3 3 95 | #define DPL_P2 2 96 | #define DPL_P1 1 97 | #define DPL_P0 0 98 | #define EN_DPL 2 99 | #define EN_ACK_PAY 1 100 | #define EN_DYN_ACK 0 101 | 102 | /* Instruction Mnemonics */ 103 | #define R_REGISTER 0x00 104 | #define W_REGISTER 0x20 105 | #define REGISTER_MASK 0x1F 106 | #define ACTIVATE 0x50 107 | #define R_RX_PL_WID 0x60 108 | #define R_RX_PAYLOAD 0x61 109 | #define W_TX_PAYLOAD 0xA0 110 | #define W_ACK_PAYLOAD 0xA8 111 | #define FLUSH_TX 0xE1 112 | #define FLUSH_RX 0xE2 113 | #define REUSE_TX_PL 0xE3 114 | #define RF24_NOP 0xFF 115 | 116 | /* Non-P omissions */ 117 | #define LNA_HCURR 0 118 | 119 | /* P model memory Map */ 120 | #define RPD 0x09 121 | #define W_TX_PAYLOAD_NO_ACK 0xB0 122 | 123 | /* P model bit Mnemonics */ 124 | #define RF_DR_LOW 5 125 | #define RF_DR_HIGH 3 126 | #define RF_PWR_LOW 1 127 | #define RF_PWR_HIGH 2 128 | -------------------------------------------------------------------------------- /HWRemoteControl/DEPRECATED/PS2_NRFTransmit/printf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2011 J. Coliz 3 | 4 | This program is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU General Public License 6 | version 2 as published by the Free Software Foundation. 7 | */ 8 | /* Galileo support from spaniakos */ 9 | 10 | /** 11 | * @file printf.h 12 | * 13 | * Setup necessary to direct stdout to the Arduino Serial library, which 14 | * enables 'printf' 15 | */ 16 | 17 | #ifndef __PRINTF_H__ 18 | #define __PRINTF_H__ 19 | 20 | #if defined (ARDUINO) && !defined (__arm__) && !defined(__ARDUINO_X86__) 21 | 22 | int serial_putc( char c, FILE * ) 23 | { 24 | Serial.write( c ); 25 | 26 | return c; 27 | } 28 | 29 | void printf_begin(void) 30 | { 31 | fdevopen( &serial_putc, 0 ); 32 | } 33 | 34 | #elif defined (__arm__) 35 | 36 | void printf_begin(void){} 37 | 38 | #elif defined(__ARDUINO_X86__) 39 | int serial_putc( char c, FILE * ) 40 | { 41 | Serial.write( c ); 42 | 43 | return c; 44 | } 45 | 46 | void printf_begin(void) 47 | { 48 | //JESUS - For reddirect stdout to /dev/ttyGS0 (Serial Monitor port) 49 | stdout = freopen("/dev/ttyGS0","w",stdout); 50 | delay(500); 51 | printf("redirecting to Serial..."); 52 | 53 | //JESUS ----------------------------------------------------------- 54 | } 55 | #else 56 | #error This example is only for use on Arduino. 57 | #endif // ARDUINO 58 | 59 | #endif // __PRINTF_H__ 60 | -------------------------------------------------------------------------------- /HWRemoteControl/ESPNOW_Telemetry/ESPNOW_Telemetry.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | uint8_t droneAddress[] = {0xD8, 0xA0, 0x1D, 0x5D, 0xFB, 0x04}; 5 | uint8_t drone2Address[] = {0xD8, 0xA0, 0x1D, 0x55, 0xB3, 0xE8}; 6 | 7 | // RF input message structure 8 | typedef struct sent_message { 9 | float loop_time; 10 | float process_time; 11 | /* 12 | float acc_x; 13 | float acc_y; 14 | float acc_z; 15 | float gyr_x; 16 | float gyr_y; 17 | float gyr_z; 18 | */ 19 | /* 20 | float pitch; 21 | float roll; 22 | float acc_z; 23 | */ 24 | int16_t roll_rate_setpoint; 25 | int16_t pitch_rate_setpoint; 26 | int16_t yaw_rate_setpoint; 27 | int32_t altitude_setpoint; 28 | float roll_rate; 29 | float pitch_rate; 30 | float yaw_rate; 31 | float altitude; 32 | } sent_message; 33 | 34 | sent_message drone_data; 35 | 36 | 37 | unsigned long prev_time = 0; 38 | float total_time = 0; 39 | 40 | // callback function that will be executed when data is received 41 | void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { 42 | 43 | bool drone1 = true; 44 | bool drone2 = true; 45 | for(uint8_t i = 0; i < 6; i++) 46 | { 47 | if(mac[i] != droneAddress[i]) 48 | { 49 | drone1 = false; 50 | break; 51 | //return; 52 | } 53 | //Serial.println(mac[i], HEX); 54 | } 55 | 56 | if(!drone1) 57 | { 58 | for(uint8_t i = 0; i < 6; i++) 59 | { 60 | if(mac[i] != drone2Address[i]) 61 | { 62 | drone2 = false; 63 | break; 64 | //return; 65 | } 66 | } 67 | } 68 | 69 | if(!drone1 && !drone2) 70 | { 71 | return; 72 | } 73 | 74 | memcpy(&drone_data, incomingData, sizeof(drone_data)); 75 | 76 | /* 77 | Serial.print(drone_data.loop_time); 78 | Serial.print(" "); 79 | Serial.println(drone_data.process_time); 80 | */ 81 | 82 | //total_time += drone_data.loop_time/1000000.0; 83 | //Serial.print(total_time, 5); 84 | //Serial.print(" "); 85 | 86 | Serial.print(drone_data.roll_rate_setpoint); 87 | Serial.print(" "); 88 | Serial.println(drone_data.roll_rate); 89 | 90 | //Serial.print(drone_data.pitch_rate_setpoint); 91 | //Serial.print(" "); 92 | //Serial.println(drone_data.pitch_rate); 93 | 94 | //Serial.print(drone_data.yaw_rate_setpoint); 95 | //Serial.print(" "); 96 | //Serial.println(drone_data.yaw_rate); 97 | 98 | //Serial.print(drone_data.altitude_setpoint); 99 | //Serial.print(" "); 100 | //Serial.println(drone_data.altitude); 101 | 102 | } 103 | 104 | void setup() { 105 | // Initialize Serial Monitor 106 | Serial.begin(115200); 107 | 108 | // Set device as a Wi-Fi Station 109 | WiFi.mode(WIFI_STA); 110 | 111 | // Init ESP-NOW 112 | if (esp_now_init() != ESP_OK) { 113 | Serial.println("Error initializing ESP-NOW"); 114 | return; 115 | } 116 | 117 | // Once ESPNow is successfully Init, we will register for recv CB to 118 | // get recv packer info 119 | esp_now_register_recv_cb(OnDataRecv); 120 | 121 | prev_time = millis(); 122 | } 123 | 124 | void loop() { 125 | } 126 | -------------------------------------------------------------------------------- /HWRemoteControl/PS2_ESPNOW_Transmit/PS2_ESPNOW_Transmit.ino: -------------------------------------------------------------------------------- 1 | #include "PsxLib.h" 2 | #include 3 | #include 4 | 5 | Psx ps2x; 6 | 7 | //uint8_t receiverAddress[] = {0x84, 0x0D, 0x8E, 0x19, 0xE4, 0xA8}; 8 | uint8_t receiverAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; // This address sends to all ESP now receivers 9 | //uint8_t receiverAddress[] = {0xD8, 0xA0, 0x1D, 0x5D, 0xFB, 0x04}; 10 | 11 | typedef struct struct_message { 12 | uint8_t hr_stick; 13 | uint8_t vr_stick; 14 | uint8_t hl_stick; 15 | uint8_t vl_stick; 16 | uint8_t start; 17 | uint8_t select; 18 | uint8_t triangle; 19 | uint8_t circle; 20 | uint8_t cross; 21 | uint8_t square; 22 | uint8_t l_stick_button; 23 | uint8_t r_stick_button; 24 | uint8_t l_back_button; 25 | uint8_t r_back_button; 26 | } struct_message; 27 | 28 | struct_message controller_data; 29 | 30 | int joystick_map(int value, float low_input, float middle_input, float high_input, float low_output, float high_output) 31 | { 32 | float middle_output = (low_output + high_output)/2.0; 33 | if(value <= middle_input) 34 | { 35 | return map(value, low_input, middle_input, low_output, middle_output); 36 | } 37 | else 38 | { 39 | return map(value, middle_input, high_input, middle_output, high_output); 40 | } 41 | } 42 | 43 | void setup() { 44 | // put your setup code here, to run once: 45 | Serial.begin(115200); 46 | Serial.println(""); 47 | Serial.println("Starting"); 48 | 49 | // Set device as a Wi-Fi Station 50 | WiFi.mode(WIFI_STA); 51 | 52 | // Init ESP-NOW 53 | if (esp_now_init() != ESP_OK) { 54 | Serial.println("Error initializing ESP-NOW"); 55 | return; 56 | } 57 | 58 | // Register peer 59 | esp_now_peer_info_t peerInfo; 60 | memcpy(peerInfo.peer_addr, receiverAddress, 6); 61 | peerInfo.channel = 0; 62 | peerInfo.encrypt = false; 63 | 64 | // Add peer 65 | if (esp_now_add_peer(&peerInfo) != ESP_OK){ 66 | Serial.println("Failed to add peer"); 67 | return; 68 | } 69 | 70 | Serial.println("ESPNow initialized correctly"); 71 | 72 | ps2x.setupPins(26, 27, 25, 32, 50); // dataPin, cmndPin, attPin, clockPin, delay 73 | Serial.println("PS2 Controller Initialized"); 74 | } 75 | 76 | void loop() { 77 | 78 | ps2x.read(); 79 | 80 | // Joystick values 81 | controller_data.hr_stick = joystick_map(ps2x.analog(0), 0, 123, 255, 0, 255); // horizontal right 82 | controller_data.vr_stick = joystick_map(ps2x.analog(1), 0, 123, 255, 255, 0); // vertical right 83 | controller_data.hl_stick = joystick_map(ps2x.analog(2), 0, 123, 255, 0, 255); // horizontal left 84 | controller_data.vl_stick = joystick_map(ps2x.analog(3), 0, 123, 255, 255, 0); // vertical left 85 | 86 | controller_data.start = ps2x.button(PSB_START); 87 | controller_data.select = ps2x.button(PSB_SELECT); 88 | 89 | controller_data.triangle = ps2x.button(PSB_TRIANGLE); 90 | controller_data.circle = ps2x.button(PSB_CIRCLE); 91 | controller_data.cross = ps2x.button(PSB_CROSS); 92 | controller_data.square = ps2x.button(PSB_SQUARE); 93 | 94 | controller_data.l_stick_button = ps2x.button(PSB_L3); // Left stick press 95 | controller_data.r_stick_button = ps2x.button(PSB_R3); // Right stick press 96 | 97 | controller_data.l_back_button = ps2x.button(PSB_L1); // Left back button 98 | controller_data.r_back_button = ps2x.button(PSB_R1); // Right back button 99 | 100 | esp_now_send(receiverAddress, (uint8_t *) &controller_data, sizeof(controller_data)); 101 | 102 | delay(20); 103 | } 104 | -------------------------------------------------------------------------------- /HWRemoteControl/PS2_ESPNOW_Transmit/PsxLib.cpp: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Simple & Compact Library (PsxLib.cpp) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Psx side connected with two R10K with Clock and Data Pins. 19 | 20 | This program is free software: you can redistribute it and/or modify 21 | it under the terms of the GNU General Public License as published by 22 | the Free Software Foundation, either version 3 of the License, or 23 | (at your option) any later version. 24 | 25 | This program is distributed in the hope that it will be useful, 26 | but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | GNU General Public License for more details. 29 | 30 | You should have received a copy of the GNU General Public License 31 | along with this program. If not, see . 32 | */ 33 | #include "PsxLib.h" 34 | 35 | static byte enter_config[]={0x01,0x43,0x00,0x01,0x00}; 36 | static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00}; 37 | static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00}; 38 | static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A}; 39 | static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01}; 40 | static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A}; 41 | 42 | Psx::Psx() { }; // Class Psx 43 | 44 | byte Psx::shift(byte _dataOut) { // Does the actual shifting, both in and out simultaneously 45 | _dataIn = 0; 46 | 47 | for (byte i = 0; i < 8; i++) { 48 | 49 | if ( _dataOut & (1 << i) ) 50 | digitalWrite(_cmndPin, HIGH); // Writes out the _dataOut bits 51 | else 52 | digitalWrite(_cmndPin, LOW); 53 | 54 | digitalWrite(_clockPin, LOW); 55 | delayMicroseconds(_delay); 56 | 57 | if ( digitalRead(_dataPin) ) { // Reads the data pin 58 | _dataIn |= _BV(i); // Giuseppe Shift the read into _dataIn (align to the others libs) 59 | } 60 | 61 | digitalWrite(_clockPin, HIGH); 62 | delayMicroseconds(_delay); 63 | } 64 | return _dataIn; 65 | } 66 | 67 | 68 | void Psx::setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay) { 69 | pinMode(dataPin, INPUT_PULLUP); 70 | //digitalWrite(dataPin, HIGH); // Turn on internal pull-up 71 | pinMode(attPin, OUTPUT); 72 | digitalWrite(_attPin, HIGH); 73 | pinMode(cmndPin, OUTPUT); 74 | pinMode(clockPin, OUTPUT); 75 | digitalWrite(_clockPin, HIGH); 76 | 77 | _dataPin = dataPin; 78 | _cmndPin = cmndPin; 79 | _attPin = attPin; 80 | _clockPin = clockPin; 81 | _delay = delay; 82 | 83 | delayMicroseconds(1000000); 84 | 85 | delayMicroseconds(1000000); 86 | 87 | 88 | 89 | do 90 | { 91 | sendCommandString(enter_config, sizeof(enter_config)); //start config run 92 | sendCommandString(set_mode, sizeof(set_mode)); 93 | sendCommandString(exit_config, sizeof(exit_config)); 94 | 95 | read(); 96 | Serial.println(_mode, HEX); // Shoujld be -x73 97 | } 98 | while(_mode & 0xf0 != 0x70); 99 | } 100 | 101 | 102 | uint32_t Psx::read() { 103 | digitalWrite(_attPin, LOW); 104 | 105 | shift(0x01); 106 | _mode = shift(0x42); 107 | shift(0xFF); 108 | 109 | _data1 = ~shift(0xFF); 110 | _data2 = ~shift(0xFF); 111 | byte _data3 = shift(0xFF); 112 | byte _data4 = shift(0xFF); 113 | byte _data5 = shift(0xFF); 114 | byte _data6 = shift(0xFF); 115 | 116 | digitalWrite(_attPin, HIGH); 117 | 118 | _last_buttons = _buttonsData; // Giuseppe: Save Old Value 119 | 120 | _buttonsData = (_data2 << 8) | _data1; 121 | _sticksData = (_data6 << 24) | (_data5 << 16) | (_data4 << 8) | _data3; 122 | 123 | return _buttonsData; 124 | } 125 | 126 | bool Psx::button(unsigned int button) { 127 | return ((_buttonsData & button) > 0); 128 | } 129 | 130 | bool Psx::newButtonState() { 131 | return ((_last_buttons ^ _buttonsData) > 0); 132 | } 133 | 134 | bool Psx::buttonNewState(unsigned int button) { 135 | return (((_last_buttons ^ _buttonsData) & button) > 0); 136 | } 137 | 138 | byte Psx::analog(byte button) { 139 | return (uint8_t) (_sticksData >> ((button)*8)); 140 | } 141 | 142 | void Psx::sendCommandString(byte string[], byte len) { 143 | 144 | digitalWrite(_attPin, LOW); 145 | 146 | for (int y=0; y < len; y++) 147 | { 148 | shift(string[y]); 149 | } 150 | digitalWrite(_attPin, HIGH); 151 | } 152 | -------------------------------------------------------------------------------- /HWRemoteControl/PS2_ESPNOW_Transmit/PsxLib.h: -------------------------------------------------------------------------------- 1 | /* PSXLIB Controller Decoder Library (PsxLib.h) 2 | To Use with PS1 and PS2 GamePad controllers with Arduino & c., Esp8266, Esp32 etc. 3 | 4 | Original written by: Kevin Ahrendt June 22nd, 2008 5 | Modified by Giuseppe Porcheddu November 5ft, 2018 to be used with Espressif ESP micro series 6 | Following also libs of Bill Porter and Kompanets Konstantin (aka I2M) 7 | 8 | * Connections: Example with Digital Pins 2, 4, 0, 15 and Ground 9 | * GamePad Cable connected to micro by five pins. 10 | * Green ACK -----------------| 11 | * Blue Clock ---- 2 Res 10K 12 | * Yellow Attn ---- 4 | 13 | * Red VCC+ ------------------ 14 | * Black Ground ---- Ground | 15 | * White Vibration NC | 16 | * Orange Command ---- 0 Res 10K 17 | * Brown Data ---- 15 ------| 18 | * Note: VCC Cable side connected with two R10K with Clock and Data Pins. 19 | 20 | Controller protocol implemented using Andrew J McCubbin's analysis. 21 | http://www.gamesx.com/controldata/psxcont/psxcont.htm 22 | 23 | Shift command is based on tutorial examples for ShiftIn and ShiftOut 24 | functions both written by Carlyn Maw and Tom Igoe 25 | http://www.arduino.cc/en/Tutorial/ShiftIn 26 | http://www.arduino.cc/en/Tutorial/ShiftOut 27 | 28 | This program is free software: you can redistribute it and/or modify 29 | it under the terms of the GNU General Public License as published by 30 | the Free Software Foundation, either version 3 of the License, or 31 | (at your option) any later version. 32 | 33 | This program is distributed in the hope that it will be useful, 34 | but WITHOUT ANY WARRANTY; without even the implied warranty of 35 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 36 | GNU General Public License for more details. 37 | 38 | You should have received a copy of the GNU General Public License 39 | along with this program. If not, see . 40 | */ 41 | 42 | 43 | #ifndef PSXLIB_H 44 | #define PSXLIB_H 45 | 46 | #if ARDUINO > 22 47 | #include "Arduino.h" 48 | #else 49 | #include "WProgram.h" 50 | #endif 51 | 52 | 53 | //These are our button constants 54 | #define PSB_IDLE 0x0000 55 | #define PSB_SELECT 0x0001 56 | #define PSB_START 0x0008 57 | #define PSB_L3 0x0002 58 | #define PSB_R3 0x0004 59 | #define PSB_L2 0x0100 60 | #define PSB_R2 0x0200 61 | #define PSB_L1 0x0400 62 | #define PSB_R1 0x0800 63 | #define PSB_GREEN 0x1000 64 | #define PSB_RED 0x2000 65 | #define PSB_BLUE 0x4000 66 | #define PSB_PINK 0x8000 67 | 68 | // Left Keys Pad and Stiks 69 | #define PSB_PAD_UP 0x0010 70 | #define PSB_PAD_RIGHT 0x0020 71 | #define PSB_PAD_DOWN 0x0040 72 | #define PSB_PAD_LEFT 0x0080 73 | 74 | #define PSB_LSTIK_LEFT 0x0010 75 | #define PSB_LSTIK_DOWN 0x0020 76 | #define PSB_LSTIK_RIGHT 0x0040 77 | #define PSB_LSTIK_UP 0x0080 78 | 79 | // Right Keys Pad and Stiks 80 | #define PSB_TRIANGLE 0x1000 81 | #define PSB_CIRCLE 0x2000 82 | #define PSB_CROSS 0x4000 83 | #define PSB_SQUARE 0x8000 84 | 85 | #define PSB_RSTIK_LEFT 0x1000 86 | #define PSB_RSTIK_DOWN 0x2000 87 | #define PSB_RSTIK_RIGHT 0x4000 88 | #define PSB_RSTIK_UP 0x8000 89 | 90 | class Psx { 91 | public: 92 | Psx(); 93 | void setupPins(byte dataPin, byte cmndPin, byte attPin, byte clockPin, byte delay); 94 | // Data Pin #, CMND Pin #, ATT Pin #, CLK Pin #, Delay 95 | // Delay is how long the clock goes without changing state in Microseconds. 96 | // It can be lowered to increase response, but if it is too low it may cause 97 | // glitches and have some keys spill over with false-positives. 98 | // A regular PSX controller works fine at 50 uSeconds. 99 | 100 | uint32_t read(); // Returns the status of the button presses in an unsignd int. 101 | // The value returned corresponds to each key as defined above. 102 | bool newButtonState(); // Giuseppe: will be true if any button was pressed 103 | bool buttonNewState(unsigned int); // Giuseppe: will be true if button changed state 104 | bool button(unsigned int); // Giuseppe: will be TRUE if button is being pressed 105 | byte analog(byte); 106 | 107 | private: 108 | void sendCommandString(byte*, byte); 109 | byte shift(byte _dataOut); 110 | 111 | byte _dataPin; 112 | byte _cmndPin; 113 | byte _attPin; 114 | byte _clockPin; 115 | 116 | byte _delay; 117 | boolean _temp; 118 | byte _dataIn; 119 | 120 | byte _mode; 121 | byte _data1; 122 | byte _data2; 123 | uint32_t _sticksData; 124 | uint32_t _buttonsData; 125 | uint32_t _last_buttons; // Giuseppe: Last Button Pressed 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 davidoises 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PIDSimulationDrone.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davidoises/FlightController/7bb12fd073a16ef1ae07648538a225fe0c13c74b/PIDSimulationDrone.slx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EW ESP32 Flight Controller 2 | 3 | This project is an **ESP32** based **Flight Controller**. This flight controller is designed to work with any quadrotor in X configuration, requiring only PID gains tuning. 4 | 5 | # The Drone 6 | This was tested on 19mmX19mm drone with tri-blade props similar to **SymaX5c**. Due to the light weight and low stiffness of these props, the sensors onboard neede **extreme low pass filters** to reduce the noise generated by the vibrations of these **unbalanced props**. Noise should decrease with higher quality props, allowing higher cut-off frecuencies for lpf's. 7 | 8 | # The codes 9 | This code was partially based on **Multiwii/Baseflight** sourcecodes, but adapted for specific sensors and functionalities required in this project. The flight control code uses **RTOS** to maintain a **constant PID sampling time**, while being able to continously receive RF control inputs. 10 | 11 | Current enabled functionalities: 12 | - Acro mode (Gyroscope-only stabilization) 13 | - Altitude hold 14 | 15 | # PID Control 16 | This repo holds the simulink file containing the control solution for this project, wich is also described in terms of implementation through the following flow chart: 17 | ![flow-chart](FlowDiagram-RTOS.jpg) 18 | 19 | # Components 20 | 21 | For basic control of attitude and altitude the flight controller operates with a **6DOF IMU** and a **barometer**. The board used in this design uses **BMX055** or **MPU6050** for orientation and **MS5611** for altitude. 22 | 23 | A **LiPo** battery is required for BLDC and ESC power. This was tested on a **3S 5200mAh** battery, but is able to support **2S** batteries. 24 | 25 | ## User Interface 26 | 27 | The board has several connectivity options, including **WiFi**, **BLE** and **950MHz** rf protocol. 28 | For WiFi and BLE, **Blynk** was used as a phone interface to control altitude and orientation. 29 | For 950MHz rf protocol, a **custom remote control** was designed. 30 | --------------------------------------------------------------------------------