├── MS5611.h ├── MPU6050.h ├── README.md ├── MS5611.c ├── MPU6050.c └── main.c /MS5611.h: -------------------------------------------------------------------------------- 1 | #ifndef MS5611_H 2 | #define MS5611_H 3 | 4 | #include "main.h" 5 | 6 | 7 | 8 | 9 | #define MS5611_ADDR 0x77 10 | 11 | #define CONVERSION_OSR_256 1 12 | #define CONVERSION_OSR_512 2 13 | #define CONVERSION_OSR_1024 3 14 | #define CONVERSION_OSR_2048 5 15 | #define CONVERSION_OSR_4096 10 16 | 17 | #define CMD_RESET 0x1E 18 | #define CMD_PROM_C0 0xA0 19 | #define CMD_PROM_C1 0xA2 20 | #define CMD_PROM_C2 0xA4 21 | #define CMD_PROM_C3 0xA6 22 | #define CMD_PROM_C4 0xA8 23 | #define CMD_PROM_C5 0xAA 24 | #define CMD_PROM_C6 0xAC 25 | #define CMD_PROM_C7 0xAE 26 | 27 | #define PRESSURE_OSR_256 0x40 28 | #define PRESSURE_OSR_512 0x42 29 | #define PRESSURE_OSR_1024 0x44 30 | #define PRESSURE_OSR_2048 0x46 31 | #define PRESSURE_OSR_4096 0x48 32 | 33 | #define TEMP_OSR_256 0x50 34 | #define TEMP_OSR_512 0x52 35 | #define TEMP_OSR_1024 0x54 36 | #define TEMP_OSR_2048 0x56 37 | #define TEMP_OSR_4096 0x58 38 | 39 | /** 40 | * @brief The oversampling rate 41 | * @warn an higher value means a longer conversion 42 | */ 43 | typedef enum OSR { 44 | OSR_256, 45 | OSR_512, 46 | OSR_1024, 47 | OSR_2048, 48 | OSR_4096 49 | }OSR; 50 | 51 | typedef struct 52 | { 53 | uint16_t C[8]; 54 | uint32_t DigitalPressure_D1; 55 | uint32_t DigitalTemperature_D2; 56 | int32_t dT; 57 | int32_t TEMP; 58 | int64_t OFF; 59 | int64_t SENS; 60 | int32_t P; 61 | 62 | float Alt; 63 | float Alt_Filt; 64 | 65 | int OFF2; 66 | int T2; 67 | int SENS2; 68 | 69 | 70 | 71 | }MS5611_t; 72 | 73 | MS5611_t MS5611; 74 | 75 | 76 | void MS5611_Reset(I2C_HandleTypeDef *I2Cx, MS5611_t *MS5611); 77 | void MS5611_ReadProm(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct); 78 | 79 | void MS5611_RequestTemperature(I2C_HandleTypeDef *I2Cx, OSR osr); 80 | void MS5611_RequestPressure(I2C_HandleTypeDef *I2Cx, OSR osr); 81 | 82 | void MS5611_ReadTemperature(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct); 83 | void MS5611_ReadPressure(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct); 84 | 85 | void MS5611_CalculateTemperature(MS5611_t *DataStruct); 86 | void MS5611_CalculatePressure(MS5611_t *DataStruct); 87 | 88 | float MS5611_getAltitude1(float pressure); 89 | float MS5611_getAltitude2(float pressure , float temperature); 90 | #endif /* MS5611_H */ 91 | -------------------------------------------------------------------------------- /MPU6050.h: -------------------------------------------------------------------------------- 1 | #ifndef MPU6050_H 2 | #define MPU6050_H 3 | 4 | //#include 5 | #include "main.h" 6 | 7 | #define D2R 0.01745329252 //Degree To Radian Constant 8 | 9 | #define MPU6050_ADDR 0xD0 //Already Left Shifted 10 | #define CONFIG_REG 0x1A 11 | #define GYRO_CONFIG_REG 0x1B 12 | #define ACCEL_CONFIG_REG 0x1C 13 | #define SMPLRT_DIV_REG 0x19 14 | #define INT_PIN_CFG 0x37 15 | #define INT_ENABLE_REG 0x38 16 | #define INT_STATUS_REG 0x3A 17 | #define ACCEL_XOUT_H_REG 0x3B 18 | #define TEMP_OUT_H_REG 0x41 19 | #define GYRO_XOUT_H_REG 0x43 20 | #define USER_CTRL_REG 0x6A 21 | #define PWR_MGMT_1_REG 0x6B 22 | #define WHO_AM_I_REG 0x75 23 | 24 | ///////////////////////////////////// 25 | #define HMC5883L_ADDRESS 0x1E // Not Left Shifted 26 | #define HMC5883L_REG_CONFIG_A (0x00) 27 | #define HMC5883L_REG_CONFIG_B (0x01) 28 | #define HMC5883L_REG_MODE (0x02) 29 | #define HMC5883L_REG_OUT_X_M (0x03) 30 | #define HMC5883L_REG_OUT_X_L (0x04) 31 | #define HMC5883L_REG_OUT_Z_M (0x05) 32 | #define HMC5883L_REG_OUT_Z_L (0x06) 33 | #define HMC5883L_REG_OUT_Y_M (0x07) 34 | #define HMC5883L_REG_OUT_Y_L (0x08) 35 | #define HMC5883L_REG_STATUS (0x09) 36 | #define HMC5883L_REG_IDENT_A (0x0A) 37 | #define HMC5883L_REG_IDENT_B (0x0B) 38 | #define HMC5883L_REG_IDENT_C (0x0C) 39 | 40 | // MPU6050 structure 41 | typedef struct _MPU6050_t 42 | { 43 | int16_t Accel_X_RAW; 44 | int16_t Accel_Y_RAW; 45 | int16_t Accel_Z_RAW; 46 | 47 | float Ax; 48 | float Ay; 49 | float Az; 50 | 51 | int16_t Gyro_X_RAW; 52 | int16_t Gyro_Y_RAW; 53 | int16_t Gyro_Z_RAW; 54 | 55 | int16_t Gyro_X_Offset; 56 | int16_t Gyro_Y_Offset; 57 | int16_t Gyro_Z_Offset; 58 | 59 | float Gx; 60 | float Gy; 61 | float Gz; 62 | 63 | float Temperature; 64 | 65 | int16_t Mag_X_RAW; 66 | int16_t Mag_Y_RAW; 67 | int16_t Mag_Z_RAW; 68 | 69 | int16_t Mag_X_Min; 70 | int16_t Mag_Y_Min; 71 | int16_t Mag_Z_Min; 72 | 73 | int16_t Mag_X_Max; 74 | int16_t Mag_Y_Max; 75 | int16_t Mag_Z_Max; 76 | 77 | int16_t Mag_X_Offset; 78 | int16_t Mag_Y_Offset; 79 | int16_t Mag_Z_Offset; 80 | 81 | float Mx; 82 | float My; 83 | float Mz; 84 | 85 | }MPU6050_t; 86 | 87 | MPU6050_t MPU6050; 88 | 89 | uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx, uint8_t Gyro_FS, uint8_t ACC_FS, uint8_t DLPF_CFG); 90 | 91 | void MPU6050_Bypass(I2C_HandleTypeDef *I2Cx); 92 | void MPU6050_Master(I2C_HandleTypeDef *I2Cx); 93 | void HMC5883L_Setup(I2C_HandleTypeDef *I2Cx); 94 | void MPU6050_Slave_Read(I2C_HandleTypeDef *I2Cx); 95 | 96 | uint8_t MPU6050_DataReady(I2C_HandleTypeDef *I2Cx); 97 | 98 | void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); 99 | void MPU6050_Read_All_DMA(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct); 100 | void MPU6050_Parsing(MPU6050_t *DataStruct); 101 | void MPU6050_Parsing_NoOffest(MPU6050_t *DataStruct); 102 | 103 | #endif /* MPU6050_H */ 104 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # About this Library 2 | The following guide is written in order to assist the use of GY-86 sensor module which combines MPU6050, HMC5883L, and MS5611 on a single PCB board. The library is intended to be used on STM32 microcontroller using HAL drivers utilizing I2C communication protocol. The overall structure of the project is inspired by leech001's library for the MPU6050. Here is the link to the repository, I hope you also check it out.
3 | https://github.com/leech001/MPU6050 4 | 5 | ## Table of Contents 6 | 1. MPU6050 & HMC5883L (Understanding Auxiliary I2C) 7 | 3. MS5611 8 | 9 | ## 1. MPU6050 & HMC5883L 10 | MPU6050 Sensor is fairly simple to use, however it is quite difficult to use it together with HMC5883L. Without understanding the auxiliary I2C, you will not be able to use HMC5883L at all.
11 | 12 | The HMC5883L sensor is not directly connected to SDA and SCL lines going through external pins. HMC5883L sensor is connected to the MPU6050 through auxiliary lines, which is physically inaccessible through external pins on the board. In order to properly set up the HMC5883L, the MPU6050 must be in bypass mode so that the micro controller can simply bypass MPU6050 and directly access to the HMC5883L. Only then the microcontroller can write registers to HMC5883L sensor. Once initialization of HMC5883L is complete, the MPU6050 should be brought back to the master mode and it should read the data from HMC5883L. The data sent by the HMC5883L will be read by the MPU6050, then it will be saved to the MPU6050's internal register address. When the user requires to read the data, the user can retrieve 9 axis values through reading the registers from MPU6050.
13 | 14 | Summary of HMC5883L
15 | 1. Set MPU6050 to bypass mode (it is set to master mode by default, therefore needs to be changed)
16 | 2. Directly access HMC5883L for initial setup
17 | 3. Set MPU6050 to back to master mode
18 | 4. Let MPU6050 to read HMC5883L through auxiliary I2C connection
19 | 5. Read 9 axis values from MPU6050
20 | 21 | Functions Explained 22 | ~~~ 23 | MPU6050_Init(I2C_HandleTypeDef *I2Cx, uint8_t Gyro_FS, uint8_t Acc_FS, uint8_t DLPF_CFG) 24 | ~~~ 25 | This function is to set the full scale of the sensor. The range of output value is always consist of 16bit, however changing the full scale will change what the value means in real physical units. 26 | 27 | ~~~ 28 | MPU6050_Bypass(I2C_HandleTypeDef *I2Cx) 29 | ~~~ 30 | This function will put the MPU6050 sensor into a bypass mode. It will basically bypass all I2C communication sent to the main I2C line towards the HMC5883L so that the MCU can directly communication with HMC5883L. The reason why you have to understand the bypass mode or master mode will be explained later in this document withing the HMC5883L section. 31 | 32 | ~~~ 33 | MPU6050_Master(I2C_HandleTypeDef *I2Cx) 34 | ~~~ 35 | This function will put the sensor back to master mode. It means that the MCU can communicate with MPU6050, but not the HMC5883L. 36 | 37 | ~~~ 38 | HMC5883L_Setup(I2C_HandleTypeDef *I2Cx) 39 | ~~~ 40 | Once the MPU6050 is set to bypass mode, use this function to complete the basic settings for the HMC5883L. 41 | 42 | ~~~ 43 | MPU6050_Slave_Read(I2C_HandleTypeDef *I2Cx) 44 | ~~~ 45 | This function will allow MPU6050 to automatically read values from HMC5883L by itself and save the data into it's own register. 46 | 47 | ~~~ 48 | MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) 49 | ~~~ 50 | This function will let the MCU read all the outputs of MPU6050 including the values that the MPU6050 has retrieved from the HMC5883L. 51 | 52 | ~~~ 53 | MPU6050_Parsing_NoOffest(MPU6050_t *DataStruct) 54 | ~~~ 55 | This function will do bit shifting so that the retrieved values from sensors would fit to the proper form, and it will also convert some values into different unit. 56 | 57 | ## 2. MS5611 58 | On the other hand, MS5611 sensor is directly connected to SDA and SCL lines so it could be accessed from the master device without any issue. 59 | 60 | -------------------------------------------------------------------------------- /MS5611.c: -------------------------------------------------------------------------------- 1 | /* 2 | * barometer.c 3 | * 4 | * Created on: 5 oct. 2018 5 | * Author: alex 6 | */ 7 | 8 | #include "MS5611.h" 9 | #include 10 | 11 | uint8_t MS5611_rx_buf[12]; 12 | uint8_t MS5611_rx_temp[3]; 13 | uint8_t MS5611_rx_press[3]; 14 | uint8_t MS5611_tx; 15 | uint8_t MS5611_rx; 16 | 17 | 18 | void MS5611_Reset(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct) 19 | { 20 | MS5611_tx = CMD_RESET; 21 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1 , &MS5611_tx, 1, 100); 22 | HAL_Delay(10); 23 | //For Temperature > 20 Celsius 24 | DataStruct->T2 = 0; 25 | DataStruct->OFF2 = 0; 26 | DataStruct->SENS2 = 0; 27 | } 28 | 29 | void MS5611_ReadProm(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct) 30 | { 31 | MS5611_tx = CMD_PROM_C0; 32 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 33 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 34 | DataStruct->C[0] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 35 | HAL_Delay(10); 36 | 37 | MS5611_tx = CMD_PROM_C1; 38 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 39 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 40 | DataStruct->C[1] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 41 | HAL_Delay(10); 42 | 43 | MS5611_tx = CMD_PROM_C2; 44 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 45 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 46 | DataStruct->C[2] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 47 | HAL_Delay(10); 48 | 49 | MS5611_tx = CMD_PROM_C3; 50 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 51 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 52 | DataStruct->C[3] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 53 | HAL_Delay(10); 54 | 55 | MS5611_tx = CMD_PROM_C4; 56 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 57 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 58 | DataStruct->C[4] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 59 | HAL_Delay(10); 60 | 61 | MS5611_tx = CMD_PROM_C5; 62 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 63 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 64 | DataStruct->C[5] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 65 | HAL_Delay(10); 66 | 67 | MS5611_tx = CMD_PROM_C6; 68 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 69 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 70 | DataStruct->C[6] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 71 | HAL_Delay(10); 72 | 73 | MS5611_tx = CMD_PROM_C7; 74 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 75 | HAL_I2C_Master_Receive(I2Cx, MS5611_ADDR << 1 , MS5611_rx_buf, 2, 100); 76 | DataStruct->C[7] = MS5611_rx_buf[0] << 8 | MS5611_rx_buf[1]; 77 | HAL_Delay(10); 78 | } 79 | 80 | 81 | void MS5611_RequestTemperature(I2C_HandleTypeDef *I2Cx, OSR osr) 82 | { 83 | MS5611_tx = TEMP_OSR_256 + (2 * osr); 84 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR <<1, &MS5611_tx, 1, 100); 85 | } 86 | 87 | void MS5611_RequestPressure(I2C_HandleTypeDef *I2Cx, OSR osr) 88 | { 89 | MS5611_tx = PRESSURE_OSR_256 + (2 * osr); 90 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 91 | } 92 | 93 | void MS5611_ReadTemperature(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct) 94 | { 95 | //Read ADC 96 | MS5611_tx = 0x00; 97 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 98 | HAL_I2C_Master_Receive(I2Cx, (MS5611_ADDR << 1) | 0x01, MS5611_rx_temp, 3, 100); 99 | // HAL_I2C_Mem_Read(I2Cx, MS5611_ADDR <<1 , 0x00, 1, MS5611_rx_temp, 3, 100); 100 | 101 | DataStruct->DigitalTemperature_D2 = (MS5611_rx_temp[0] << 16) | (MS5611_rx_temp[1] << 8) | MS5611_rx_temp[2]; 102 | } 103 | 104 | void MS5611_ReadPressure(I2C_HandleTypeDef *I2Cx, MS5611_t *DataStruct) 105 | { 106 | //Read ADC 107 | MS5611_tx = 0x00; 108 | HAL_I2C_Master_Transmit(I2Cx, MS5611_ADDR << 1, &MS5611_tx, 1, 100); 109 | HAL_I2C_Master_Receive(I2Cx, (MS5611_ADDR << 1) | 0x01, MS5611_rx_press, 3, 100); 110 | //HAL_I2C_Mem_Read(I2Cx, MS5611_ADDR <<1, 0x00, 1, MS5611_rx_press, 3, 100); 111 | 112 | DataStruct->DigitalPressure_D1 = MS5611_rx_press[0] << 16 | MS5611_rx_press[1] << 8 | MS5611_rx_press[2]; 113 | } 114 | 115 | void MS5611_CalculateTemperature(MS5611_t *DataStruct) 116 | { 117 | DataStruct->dT = DataStruct->C[5]; 118 | DataStruct->dT <<= 8; //Calculated up to C5 * 2^8 119 | DataStruct->dT *= -1; //Apply negative sign 120 | DataStruct->dT += DataStruct->DigitalTemperature_D2; // = D2 - C5 * 2^8 121 | 122 | DataStruct->TEMP = DataStruct->dT * DataStruct->C[6]; 123 | DataStruct->TEMP >>= 23; // Calculated up to dT * C6 / 2^23 124 | DataStruct->TEMP += 2000; 125 | } 126 | 127 | void MS5611_CalculatePressure(MS5611_t *DataStruct) 128 | { 129 | DataStruct->OFF = DataStruct->C[2]; 130 | DataStruct->OFF <<= 16; //Calculated up to C2 * 2^16 131 | DataStruct->OFF += (DataStruct->C[4] * DataStruct->dT) >> 7; 132 | 133 | 134 | DataStruct->SENS = DataStruct->C[1]; 135 | DataStruct->SENS <<= 15; // Calculated up to C1 * 2^15 136 | DataStruct->SENS += (DataStruct->C[3] * DataStruct->dT) >>8; 137 | 138 | DataStruct->P = ((DataStruct->DigitalPressure_D1 * DataStruct->SENS) / pow(2, 21) - DataStruct->OFF) / pow(2, 15); 139 | } 140 | 141 | #define SEA_PRESSURE 1013.25f 142 | 143 | float MS5611_getAltitude1(float pressure) //No temperature correction. 144 | { 145 | return (1.0f - powf((pressure / SEA_PRESSURE), 0.1902226f)) * 44307.69396f; //145366.45f * 0.3048f = 44307.69396f; 146 | } 147 | 148 | float MS5611_getAltitude2(float pressure, float temperature) //Get Altitude with temperature correction. 149 | { 150 | return (1.0f - powf((pressure / SEA_PRESSURE), 0.1902226f)) * (temperature + 273.15f) / 0.0065f; 151 | } 152 | 153 | -------------------------------------------------------------------------------- /MPU6050.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mpu6050.h" 3 | 4 | uint8_t MPU6050_rx; 5 | uint8_t MPU6050_rx_buf[20]; 6 | uint8_t MPU6050_tx; 7 | float MPU6050_Gyro_LSB = 32.8; 8 | float MPU6050_Acc_LSB = 4096.0; 9 | 10 | uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx, uint8_t Gyro_FS, uint8_t Acc_FS, uint8_t DLPF_CFG) 11 | { 12 | //Save LSB/Unit for both gyro and acc in order to use them later 13 | switch(Gyro_FS) 14 | { 15 | case 0: //250dps 16 | MPU6050_Gyro_LSB = 131.0; 17 | break; 18 | case 1: //500dps 19 | MPU6050_Gyro_LSB = 65.5; 20 | break; 21 | case 2: //1000dps 22 | MPU6050_Gyro_LSB = 32.8; 23 | break; 24 | case 3: //2000dps 25 | MPU6050_Gyro_LSB = 16.4; 26 | break; 27 | default: 28 | break; 29 | } 30 | 31 | switch(Acc_FS) 32 | { 33 | case 0: //2g 34 | MPU6050_Acc_LSB = 16384.0; 35 | break; 36 | case 1: //4g 37 | MPU6050_Acc_LSB = 8192.0; 38 | break; 39 | case 2: //8g 40 | MPU6050_Acc_LSB = 4096.0; 41 | break; 42 | case 3: //16g 43 | MPU6050_Acc_LSB = 2048.0; 44 | break; 45 | default: 46 | break; 47 | } 48 | 49 | // Read Who am I 50 | HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &MPU6050_rx, 1, 100); 51 | MPU6050_tx = 0; //Will return this value if code ends here 52 | 53 | // 0x68 will be returned if sensor accessed correctly 54 | if (MPU6050_rx == 0x68) 55 | { 56 | MPU6050_tx = 0; 57 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &MPU6050_tx, 1, 100); 58 | HAL_Delay(10); 59 | 60 | MPU6050_tx = 0x00; // Set No Sampling 61 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &MPU6050_tx, 1, 100); 62 | HAL_Delay(10); 63 | 64 | MPU6050_tx = DLPF_CFG; // Digital Low Pass Filter Setting 65 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, CONFIG_REG, 1, &MPU6050_tx, 1, 100); 66 | HAL_Delay(10); 67 | 68 | MPU6050_tx = Gyro_FS << 3; 69 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &MPU6050_tx, 1, 100); 70 | HAL_Delay(10); 71 | 72 | MPU6050_tx = Acc_FS << 3; 73 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &MPU6050_tx, 1, 100); 74 | HAL_Delay(10); 75 | 76 | return 0; 77 | } 78 | return 1; 79 | } 80 | 81 | void MPU6050_Bypass(I2C_HandleTypeDef *I2Cx) 82 | { 83 | MPU6050_tx = 0b00000000; // 84 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x6A, 1, &MPU6050_tx, 1, 100); //Master Disable 85 | HAL_Delay(10); 86 | 87 | MPU6050_tx = 0b00000010; // 88 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x37, 1, &MPU6050_tx, 1, 100); //Bypass Enable 89 | HAL_Delay(10); 90 | } 91 | 92 | void MPU6050_Master(I2C_HandleTypeDef *I2Cx) 93 | { 94 | MPU6050_tx = 0x00; // 95 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x37, 1, &MPU6050_tx, 1, 100); //Disable Bypass 96 | HAL_Delay(10); 97 | 98 | MPU6050_tx = 0b00100010; // 99 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x6A, 1, &MPU6050_tx, 1, 100); //Master Enable 100 | HAL_Delay(10); 101 | 102 | MPU6050_tx = 0b00001101; // 103 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x24, 1, &MPU6050_tx, 1, 100); //Master Clock to 400kHz 104 | HAL_Delay(10); 105 | 106 | MPU6050_tx = 0x00; 107 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &MPU6050_tx, 1, 100); 108 | HAL_Delay(10); 109 | } 110 | 111 | void HMC5883L_Setup(I2C_HandleTypeDef *I2Cx) 112 | { 113 | MPU6050_tx = 0b00011000; //Fill Slave0 DO 114 | HAL_I2C_Mem_Write(I2Cx, HMC5883L_ADDRESS << 1, 0x00, 1, &MPU6050_tx, 1, 100); 115 | HAL_Delay(10); 116 | 117 | MPU6050_tx = 0b00100000; //Fill Slave0 DO 118 | HAL_I2C_Mem_Write(I2Cx, HMC5883L_ADDRESS << 1, 0x01, 1, &MPU6050_tx, 1, 100); 119 | HAL_Delay(10); 120 | 121 | MPU6050_tx = 0x00; 122 | HAL_I2C_Mem_Write(I2Cx, HMC5883L_ADDRESS << 1, 0x02, 1, &MPU6050_tx, 1, 100); //Mode: Continuous 123 | HAL_Delay(10); 124 | } 125 | 126 | void MPU6050_Slave_Read(I2C_HandleTypeDef *I2Cx) 127 | { 128 | MPU6050_tx = HMC5883L_ADDRESS | 0x80; //Access Slave into read mode 129 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x25, 1, &MPU6050_tx, 1, 100); 130 | HAL_Delay(10); 131 | 132 | MPU6050_tx = 0x03; //Slave REG for reading to take place 133 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x26, 1, &MPU6050_tx, 1, 100); 134 | HAL_Delay(10); 135 | 136 | MPU6050_tx = 0x80 | 0x06; //Number of data bytes 137 | HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, 0x27, 1, &MPU6050_tx, 1, 100); 138 | HAL_Delay(10); 139 | } 140 | 141 | uint8_t MPU6050_DataReady(I2C_HandleTypeDef *I2Cx) 142 | { 143 | HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, INT_STATUS_REG, 1, &MPU6050_rx, 1, 100); 144 | return MPU6050_rx; 145 | } 146 | 147 | void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) 148 | { 149 | HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, MPU6050_rx_buf, 20, 100); 150 | } 151 | 152 | void MPU6050_Read_All_DMA(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct) 153 | { 154 | HAL_I2C_Mem_Read_DMA(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, MPU6050_rx_buf, 20); 155 | } 156 | 157 | void MPU6050_Parsing(MPU6050_t *DataStruct) 158 | { 159 | DataStruct->Accel_X_RAW = -(MPU6050_rx_buf[0] << 8 | MPU6050_rx_buf[1]); 160 | DataStruct->Accel_Y_RAW = (MPU6050_rx_buf[2] << 8 | MPU6050_rx_buf[3]); 161 | DataStruct->Accel_Z_RAW = (MPU6050_rx_buf[4] << 8 | MPU6050_rx_buf[5]); 162 | // Didn't Save Temp Value 163 | DataStruct->Gyro_X_RAW = (MPU6050_rx_buf[8] << 8 | MPU6050_rx_buf[9]); 164 | DataStruct->Gyro_Y_RAW = -(MPU6050_rx_buf[10] << 8 | MPU6050_rx_buf[11]); 165 | DataStruct->Gyro_Z_RAW = -(MPU6050_rx_buf[12] << 8 | MPU6050_rx_buf[13]); 166 | 167 | DataStruct->Mag_X_RAW = (MPU6050_rx_buf[14] << 8 | MPU6050_rx_buf[15]); 168 | DataStruct->Mag_Z_RAW = -(MPU6050_rx_buf[16] << 8 | MPU6050_rx_buf[17]); 169 | DataStruct->Mag_Y_RAW = -(MPU6050_rx_buf[18] << 8 | MPU6050_rx_buf[19]); 170 | 171 | DataStruct->Gyro_X_RAW -= DataStruct->Gyro_X_Offset; 172 | DataStruct->Gyro_Y_RAW -= DataStruct->Gyro_Y_Offset; 173 | DataStruct->Gyro_Z_RAW -= DataStruct->Gyro_Z_Offset; 174 | 175 | DataStruct->Mag_X_RAW -= DataStruct->Mag_X_Offset; 176 | DataStruct->Mag_Y_RAW -= DataStruct->Mag_Y_Offset; 177 | DataStruct->Mag_Z_RAW -= DataStruct->Mag_Z_Offset; 178 | 179 | 180 | DataStruct->Gx = DataStruct->Gyro_X_RAW / MPU6050_Gyro_LSB* D2R; 181 | DataStruct->Gy = DataStruct->Gyro_Y_RAW / MPU6050_Gyro_LSB* D2R; 182 | DataStruct->Gz = DataStruct->Gyro_Z_RAW / MPU6050_Gyro_LSB* D2R; 183 | DataStruct->Ax = DataStruct->Accel_X_RAW / MPU6050_Acc_LSB; 184 | DataStruct->Ay = DataStruct->Accel_Y_RAW / MPU6050_Acc_LSB; 185 | DataStruct->Az = DataStruct->Accel_Z_RAW / MPU6050_Acc_LSB; 186 | } 187 | 188 | void MPU6050_Parsing_NoOffest(MPU6050_t *DataStruct) 189 | { 190 | DataStruct->Accel_X_RAW = -(MPU6050_rx_buf[0] << 8 | MPU6050_rx_buf[1]); 191 | DataStruct->Accel_Y_RAW = (MPU6050_rx_buf[2] << 8 | MPU6050_rx_buf[3]); 192 | DataStruct->Accel_Z_RAW = (MPU6050_rx_buf[4] << 8 | MPU6050_rx_buf[5]); 193 | // Didn't Save Temp Value 194 | DataStruct->Gyro_X_RAW = (MPU6050_rx_buf[8] << 8 | MPU6050_rx_buf[9]); 195 | DataStruct->Gyro_Y_RAW = -(MPU6050_rx_buf[10] << 8 | MPU6050_rx_buf[11]); 196 | DataStruct->Gyro_Z_RAW = -(MPU6050_rx_buf[12] << 8 | MPU6050_rx_buf[13]); 197 | 198 | DataStruct->Mag_X_RAW = (MPU6050_rx_buf[14] << 8 | MPU6050_rx_buf[15]); 199 | DataStruct->Mag_Z_RAW = -(MPU6050_rx_buf[16] << 8 | MPU6050_rx_buf[17]); 200 | DataStruct->Mag_Y_RAW = -(MPU6050_rx_buf[18] << 8 | MPU6050_rx_buf[19]); 201 | 202 | DataStruct->Gyro_X_RAW -= DataStruct->Gyro_X_Offset; 203 | DataStruct->Gyro_Y_RAW -= DataStruct->Gyro_Y_Offset; 204 | DataStruct->Gyro_Z_RAW -= DataStruct->Gyro_Z_Offset; 205 | 206 | 207 | DataStruct->Gx = DataStruct->Gyro_X_RAW / MPU6050_Gyro_LSB* D2R; 208 | DataStruct->Gy = DataStruct->Gyro_Y_RAW / MPU6050_Gyro_LSB* D2R; 209 | DataStruct->Gz = DataStruct->Gyro_Z_RAW / MPU6050_Gyro_LSB* D2R; 210 | DataStruct->Ax = DataStruct->Accel_X_RAW / MPU6050_Acc_LSB; 211 | DataStruct->Ay = DataStruct->Accel_Y_RAW / MPU6050_Acc_LSB; 212 | DataStruct->Az = DataStruct->Accel_Z_RAW / MPU6050_Acc_LSB; 213 | } 214 | -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : main.c 5 | * @brief : Main program body 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | *

© Copyright (c) 2021 STMicroelectronics. 10 | * All rights reserved.

11 | * 12 | * This software component is licensed by ST under BSD 3-Clause license, 13 | * the "License"; You may not use this file except in compliance with the 14 | * License. You may obtain a copy of the License at: 15 | * opensource.org/licenses/BSD-3-Clause 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Includes ------------------------------------------------------------------*/ 21 | #include "main.h" 22 | 23 | /* Private includes ----------------------------------------------------------*/ 24 | /* USER CODE BEGIN Includes */ 25 | #include "MPU6050.h" 26 | #include "MadgwickAHRS.h" 27 | #include "FS-iA6B.h" 28 | #include "pid control.h" 29 | #include "w25qxx.h" 30 | #include "MS5611.h" 31 | 32 | /* USER CODE END Includes */ 33 | 34 | /* Private typedef -----------------------------------------------------------*/ 35 | /* USER CODE BEGIN PTD */ 36 | 37 | /* USER CODE END PTD */ 38 | 39 | /* Private define ------------------------------------------------------------*/ 40 | /* USER CODE BEGIN PD */ 41 | /* USER CODE END PD */ 42 | 43 | /* Private macro -------------------------------------------------------------*/ 44 | /* USER CODE BEGIN PM */ 45 | 46 | /* USER CODE END PM */ 47 | 48 | /* Private variables ---------------------------------------------------------*/ 49 | I2C_HandleTypeDef hi2c1; 50 | DMA_HandleTypeDef hdma_i2c1_rx; 51 | 52 | SPI_HandleTypeDef hspi3; 53 | 54 | TIM_HandleTypeDef htim5; 55 | 56 | UART_HandleTypeDef huart4; 57 | UART_HandleTypeDef huart1; 58 | UART_HandleTypeDef huart3; 59 | UART_HandleTypeDef huart6; 60 | DMA_HandleTypeDef hdma_usart3_tx; 61 | DMA_HandleTypeDef hdma_usart3_rx; 62 | 63 | /* USER CODE BEGIN PV */ 64 | 65 | 66 | // Timer variables 67 | extern uint8_t tim7_1ms_flag; 68 | extern uint8_t tim7_10ms_flag; 69 | extern uint8_t tim7_100ms_flag; 70 | extern uint8_t tim7_200ms_flag; 71 | extern uint8_t tim7_500ms_flag; 72 | extern uint8_t tim7_1000ms_flag; 73 | extern float MPU6050_Roll; 74 | extern float MPU6050_Yaw; 75 | extern float MPU6050_Pitch; 76 | extern uint8_t ibus_rx_cplt_flag; 77 | extern uint8_t ibus_rx_buf[32]; 78 | extern uint8_t uart3_rx_data; 79 | 80 | unsigned char failsafe_flag = 0; 81 | unsigned int ccr1 ,ccr2, ccr3, ccr4; 82 | 83 | unsigned char telemetry_tx_buf[35]; 84 | 85 | unsigned char Mag_Calib[6] = {0}; 86 | 87 | unsigned short iBus_SwA_Prev = 0; 88 | unsigned char iBus_rx_cnt = 0; 89 | unsigned char iBus_VrA_flag = 0; 90 | unsigned char iBus_VrA_Prev_flag = 0; 91 | unsigned char iBus_VrB_flag = 0; 92 | unsigned char iBus_VrB_Prev_flag = 0; 93 | 94 | unsigned char motor_arming_flag = 0; 95 | 96 | unsigned char is_throttle_middle = 0; 97 | unsigned char is_yaw_middle = 0; 98 | 99 | float yaw_heading_reference = 0; 100 | float altbaro = 0.0; 101 | 102 | 103 | 104 | float alt_arr[10] = {0}; 105 | int alt_cnt = 0; 106 | 107 | int mpu_read_busy = 0; 108 | /* USER CODE END PV */ 109 | 110 | /* Private function prototypes -----------------------------------------------*/ 111 | void SystemClock_Config(void); 112 | static void MX_GPIO_Init(void); 113 | static void MX_DMA_Init(void); 114 | static void MX_TIM7_Init(void); 115 | static void MX_UART5_Init(void); 116 | static void MX_TIM5_Init(void); 117 | static void MX_USART3_UART_Init(void); 118 | static void MX_SPI3_Init(void); 119 | static void MX_I2C1_Init(void); 120 | static void MX_UART4_Init(void); 121 | static void MX_USART1_UART_Init(void); 122 | static void MX_USART6_UART_Init(void); 123 | /* USER CODE BEGIN PFP */ 124 | int _write(int file, char* p, int len) 125 | { 126 | // for(int i=0;i MPU6050.Mag_X_Max) MPU6050.Mag_X_Max = MPU6050.Mag_X_RAW; 240 | if(MPU6050.Mag_X_RAW < MPU6050.Mag_X_Min) MPU6050.Mag_X_Min = MPU6050.Mag_X_RAW; 241 | 242 | if(MPU6050.Mag_Y_RAW > MPU6050.Mag_Y_Max) MPU6050.Mag_Y_Max = MPU6050.Mag_Y_RAW; 243 | if(MPU6050.Mag_Y_RAW < MPU6050.Mag_Y_Min) MPU6050.Mag_Y_Min = MPU6050.Mag_Y_RAW; 244 | 245 | if(MPU6050.Mag_Z_RAW > MPU6050.Mag_Z_Max) MPU6050.Mag_Z_Max = MPU6050.Mag_Z_RAW; 246 | if(MPU6050.Mag_Z_RAW < MPU6050.Mag_Z_Min) MPU6050.Mag_Z_Min = MPU6050.Mag_Z_RAW; 247 | HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_8); 248 | } 249 | MPU6050.Mag_X_Offset = (MPU6050.Mag_X_Max + MPU6050.Mag_X_Min) / 2; 250 | MPU6050.Mag_Y_Offset = (MPU6050.Mag_Y_Max + MPU6050.Mag_Y_Min) / 2; 251 | MPU6050.Mag_Z_Offset = (MPU6050.Mag_Z_Max + MPU6050.Mag_Z_Min) / 2; 252 | 253 | Mag_Calib[0] = MPU6050.Mag_X_Offset >> 8; 254 | Mag_Calib[1] = MPU6050.Mag_X_Offset; 255 | Mag_Calib[2] = MPU6050.Mag_Y_Offset >> 8; 256 | Mag_Calib[3] = MPU6050.Mag_Y_Offset; 257 | Mag_Calib[4] = MPU6050.Mag_Z_Offset >> 8; 258 | Mag_Calib[5] = MPU6050.Mag_Z_Offset; 259 | 260 | W25qxx_WritePage(Mag_Calib, 0, 0, 6); 261 | } 262 | 263 | HAL_Delay(200); 264 | W25qxx_ReadPage(Mag_Calib, 0, 0, 6); 265 | MPU6050.Mag_X_Offset = Mag_Calib[0] << 8 | Mag_Calib[1]; 266 | MPU6050.Mag_Y_Offset = Mag_Calib[2] << 8 | Mag_Calib[3]; 267 | MPU6050.Mag_Z_Offset = Mag_Calib[4] << 8 | Mag_Calib[5]; 268 | 269 | //ESC Calibration 270 | if(iBus.SwC == 2000) 271 | { 272 | ESC_Calibration(); 273 | while(iBus.SwC != 1000) 274 | { 275 | Is_iBus_Received(); 276 | LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH4); 277 | TIM3->PSC = 1500; 278 | HAL_Delay(200); 279 | TIM3->PSC = 2000; 280 | HAL_Delay(200); 281 | LL_TIM_CC_DisableChannel(TIM3, LL_TIM_CHANNEL_CH4); 282 | } 283 | } 284 | /* USER CODE END 2 */ 285 | 286 | /* Infinite loop */ 287 | /* USER CODE BEGIN WHILE */ 288 | 289 | while (1) 290 | { 291 | /* USER CODE END WHILE */ 292 | 293 | /* USER CODE BEGIN 3 */ 294 | if(ibus_rx_cplt_flag==1) 295 | { 296 | ibus_rx_cplt_flag =0; 297 | if(iBus_Check_CHKSUM(ibus_rx_buf, 32)) 298 | { 299 | iBus_Parsing(ibus_rx_buf , &iBus); 300 | } 301 | } 302 | 303 | //if(tim7_10ms_flag==0 && tim7_1ms_flag==1) 304 | if(tim7_1ms_flag==1) 305 | { 306 | tim7_1ms_flag = 0; 307 | mpu_read_busy = 1; 308 | MPU6050_Read_All_DMA(&hi2c1, &MPU6050); 309 | MPU6050_Parsing(&MPU6050); 310 | MadgwickAHRSupdate(MPU6050.Gx, MPU6050.Gy, MPU6050.Gz, MPU6050.Ax, MPU6050.Ay, MPU6050.Az,MPU6050.Mag_X_RAW , MPU6050.Mag_Y_RAW,MPU6050.Mag_Z_RAW); 311 | 312 | Double_Roll_Pitch_PID_Calculation(&pitch, -(iBus.RV - 1500)*0.07f, MPU6050_Pitch, MPU6050.Gx); 313 | Double_Roll_Pitch_PID_Calculation(&roll, (iBus.RH - 1500)*0.07f, MPU6050_Roll, MPU6050.Gy); 314 | 315 | if(iBus.LH > 1485 && iBus.LH < 1515) is_yaw_middle = 1; 316 | else is_yaw_middle = 0; 317 | // HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_8); 318 | 319 | if(is_yaw_middle == 0) 320 | { 321 | yaw_heading_reference = MPU6050_Yaw; 322 | Single_Yaw_Rate_PID_Calculation(&yaw_rate, (iBus.LH-1500), MPU6050.Gz); 323 | ccr1 = 84000 + (iBus.LV - 1000) * 83.9 + pitch.in.pid_result - roll.in.pid_result - yaw_rate.pid_result; 324 | ccr2 = 84000 + (iBus.LV - 1000) * 83.9 - pitch.in.pid_result - roll.in.pid_result + yaw_rate.pid_result; 325 | ccr3 = 84000 + (iBus.LV - 1000) * 83.9 - pitch.in.pid_result + roll.in.pid_result - yaw_rate.pid_result; 326 | ccr4 = 84000 + (iBus.LV - 1000) * 83.9 + pitch.in.pid_result + roll.in.pid_result + yaw_rate.pid_result; 327 | } 328 | else 329 | { 330 | Double_Yaw_Heading_PID_Calculation(&yaw_heading, yaw_heading_reference, MPU6050_Yaw, MPU6050.Gz); 331 | ccr1 = 84000 + (iBus.LV - 1000) * 83.9 + pitch.in.pid_result - roll.in.pid_result - yaw_heading.in.pid_result; 332 | ccr2 = 84000 + (iBus.LV - 1000) * 83.9 - pitch.in.pid_result - roll.in.pid_result + yaw_heading.in.pid_result; 333 | ccr3 = 84000 + (iBus.LV - 1000) * 83.9 - pitch.in.pid_result + roll.in.pid_result - yaw_heading.in.pid_result; 334 | ccr4 = 84000 + (iBus.LV - 1000) * 83.9 + pitch.in.pid_result + roll.in.pid_result + yaw_heading.in.pid_result; 335 | } 336 | } 337 | // else if(tim7_10ms_flag==1 && mpu_read_busy == 0) 338 | if(tim7_10ms_flag==1 && mpu_read_busy == 0) 339 | { 340 | tim7_10ms_flag=0; 341 | 342 | if(alt_cnt == 0) 343 | { 344 | MS5611_RequestTemperature(&hi2c1,OSR_4096); 345 | alt_cnt = 1; 346 | } 347 | 348 | else if (alt_cnt == 1) 349 | { 350 | MS5611_ReadTemperature(&hi2c1,&MS5611); 351 | MS5611_CalculateTemperature(&MS5611); 352 | MS5611_RequestPressure(&hi2c1, OSR_4096); 353 | alt_cnt = 2; 354 | } 355 | else 356 | { 357 | MS5611_ReadPressure(&hi2c1,&MS5611); 358 | MS5611_CalculatePressure(&MS5611); 359 | MS5611_RequestTemperature(&hi2c1,OSR_4096); 360 | MS5611.Alt = (MS5611_getAltitude1((float)MS5611.P/100.f))*100; 361 | 362 | #define X 0.90f 363 | MS5611.Alt_Filt = MS5611.Alt_Filt * X + MS5611.Alt * (1.0f-X); 364 | alt_cnt = 1; 365 | 366 | // printf("%.1f \t %.1f \n", MS5611.Alt, MS5611.Alt_Filt); 367 | printf("%.1f \t %.1f %.1f \t \n", MPU6050_Roll, MPU6050_Pitch, MPU6050_Yaw); 368 | 369 | // printf("%d \t %.d\t %d \t %d \t %d \t %d \t \n", MPU6050.Mag_X_RAW,MPU6050.Mag_Y_RAW,MPU6050.Mag_Z_RAW, MPU6050.Mag_X_Offset,MPU6050.Mag_Y_Offset, MPU6050.Mag_Z_Offset); 370 | // printf("%d \t %d \t %d \t \n", MPU6050.Mag_X_RAW,MPU6050.Mag_Y_RAW,MPU6050.Mag_Z_RAW); 371 | // printf("%.1f \t %.1f %.1f \t %.1f \t \n", MPU6050_Roll, MPU6050_Pitch, MPU6050_Yaw , MS5611.Alt_Filt); 372 | } 373 | } 374 | 375 | if(iBus.LV < 1030 || motor_arming_flag == 0) 376 | { 377 | Reset_All_PID_Integrator(); 378 | } 379 | 380 | if(iBus.SwA == 2000 && iBus_SwA_Prev != 2000) 381 | { 382 | if(iBus.LV < 1010) 383 | { 384 | motor_arming_flag = 1; 385 | yaw_heading_reference = MPU6050_Yaw; 386 | } 387 | } 388 | iBus_SwA_Prev = iBus.SwA; 389 | 390 | if(iBus.SwA != 2000) 391 | { 392 | motor_arming_flag = 0; 393 | } 394 | if(motor_arming_flag == 1) 395 | { 396 | if(failsafe_flag == 0) 397 | { 398 | if(iBus.LV > 1050) 399 | { 400 | TIM5->CCR1 = ccr1 > 167900 ? 167900 : ccr1 < 84000 ? 84000 : ccr1; 401 | TIM5->CCR2 = ccr2 > 167900 ? 167900 : ccr2 < 84000 ? 84000 : ccr2; 402 | TIM5->CCR3 = ccr3 > 167900 ? 167900 : ccr3 < 84000 ? 84000 : ccr3; 403 | TIM5->CCR4 = ccr4 > 167900 ? 167900 : ccr4 < 84000 ? 84000 : ccr4; 404 | 405 | } 406 | else 407 | { 408 | TIM5->CCR1 = 84000; 409 | TIM5->CCR2 = 84000; 410 | TIM5->CCR3 = 84000; 411 | TIM5->CCR4 = 84000; 412 | } 413 | } 414 | else 415 | { 416 | TIM5->CCR1 = 84000; 417 | TIM5->CCR2 = 84000; 418 | TIM5->CCR3 = 84000; 419 | TIM5->CCR4 = 84000; 420 | } 421 | } 422 | else 423 | { 424 | TIM5->CCR1 = 84000; 425 | TIM5->CCR2 = 84000; 426 | TIM5->CCR3 = 84000; 427 | TIM5->CCR4 = 84000; 428 | } 429 | 430 | } 431 | 432 | /* USER CODE END 3 */ 433 | } 434 | 435 | /** 436 | * @brief System Clock Configuration 437 | * @retval None 438 | */ 439 | void SystemClock_Config(void) 440 | { 441 | RCC_OscInitTypeDef RCC_OscInitStruct = {0}; 442 | RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; 443 | 444 | /** Configure the main internal regulator output voltage 445 | */ 446 | __HAL_RCC_PWR_CLK_ENABLE(); 447 | __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); 448 | /** Initializes the RCC Oscillators according to the specified parameters 449 | * in the RCC_OscInitTypeDef structure. 450 | */ 451 | RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; 452 | RCC_OscInitStruct.HSEState = RCC_HSE_ON; 453 | RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; 454 | RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; 455 | RCC_OscInitStruct.PLL.PLLM = 4; 456 | RCC_OscInitStruct.PLL.PLLN = 168; 457 | RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; 458 | RCC_OscInitStruct.PLL.PLLQ = 4; 459 | if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) 460 | { 461 | Error_Handler(); 462 | } 463 | /** Initializes the CPU, AHB and APB buses clocks 464 | */ 465 | RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK 466 | |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; 467 | RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; 468 | RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; 469 | RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; 470 | RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; 471 | 472 | if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) 473 | { 474 | Error_Handler(); 475 | } 476 | } 477 | 478 | /** 479 | * @brief I2C1 Initialization Function 480 | * @param None 481 | * @retval None 482 | */ 483 | static void MX_I2C1_Init(void) 484 | { 485 | 486 | /* USER CODE BEGIN I2C1_Init 0 */ 487 | 488 | /* USER CODE END I2C1_Init 0 */ 489 | 490 | /* USER CODE BEGIN I2C1_Init 1 */ 491 | 492 | /* USER CODE END I2C1_Init 1 */ 493 | hi2c1.Instance = I2C1; 494 | hi2c1.Init.ClockSpeed = 400000; 495 | hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; 496 | hi2c1.Init.OwnAddress1 = 0; 497 | hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; 498 | hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; 499 | hi2c1.Init.OwnAddress2 = 0; 500 | hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; 501 | hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; 502 | if (HAL_I2C_Init(&hi2c1) != HAL_OK) 503 | { 504 | Error_Handler(); 505 | } 506 | /* USER CODE BEGIN I2C1_Init 2 */ 507 | 508 | /* USER CODE END I2C1_Init 2 */ 509 | 510 | } 511 | 512 | /** 513 | * @brief SPI3 Initialization Function 514 | * @param None 515 | * @retval None 516 | */ 517 | static void MX_SPI3_Init(void) 518 | { 519 | 520 | /* USER CODE BEGIN SPI3_Init 0 */ 521 | 522 | /* USER CODE END SPI3_Init 0 */ 523 | 524 | /* USER CODE BEGIN SPI3_Init 1 */ 525 | 526 | /* USER CODE END SPI3_Init 1 */ 527 | /* SPI3 parameter configuration*/ 528 | hspi3.Instance = SPI3; 529 | hspi3.Init.Mode = SPI_MODE_MASTER; 530 | hspi3.Init.Direction = SPI_DIRECTION_2LINES; 531 | hspi3.Init.DataSize = SPI_DATASIZE_8BIT; 532 | hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; 533 | hspi3.Init.CLKPhase = SPI_PHASE_1EDGE; 534 | hspi3.Init.NSS = SPI_NSS_SOFT; 535 | hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; 536 | hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB; 537 | hspi3.Init.TIMode = SPI_TIMODE_DISABLE; 538 | hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; 539 | hspi3.Init.CRCPolynomial = 10; 540 | if (HAL_SPI_Init(&hspi3) != HAL_OK) 541 | { 542 | Error_Handler(); 543 | } 544 | /* USER CODE BEGIN SPI3_Init 2 */ 545 | 546 | /* USER CODE END SPI3_Init 2 */ 547 | 548 | } 549 | 550 | /** 551 | * @brief TIM5 Initialization Function 552 | * @param None 553 | * @retval None 554 | */ 555 | static void MX_TIM5_Init(void) 556 | { 557 | 558 | /* USER CODE BEGIN TIM5_Init 0 */ 559 | 560 | /* USER CODE END TIM5_Init 0 */ 561 | 562 | TIM_ClockConfigTypeDef sClockSourceConfig = {0}; 563 | TIM_MasterConfigTypeDef sMasterConfig = {0}; 564 | TIM_OC_InitTypeDef sConfigOC = {0}; 565 | 566 | /* USER CODE BEGIN TIM5_Init 1 */ 567 | 568 | /* USER CODE END TIM5_Init 1 */ 569 | htim5.Instance = TIM5; 570 | htim5.Init.Prescaler = 0; 571 | htim5.Init.CounterMode = TIM_COUNTERMODE_UP; 572 | htim5.Init.Period = 167999; 573 | htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; 574 | htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; 575 | if (HAL_TIM_Base_Init(&htim5) != HAL_OK) 576 | { 577 | Error_Handler(); 578 | } 579 | sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; 580 | if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK) 581 | { 582 | Error_Handler(); 583 | } 584 | if (HAL_TIM_PWM_Init(&htim5) != HAL_OK) 585 | { 586 | Error_Handler(); 587 | } 588 | sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; 589 | sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; 590 | if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) 591 | { 592 | Error_Handler(); 593 | } 594 | sConfigOC.OCMode = TIM_OCMODE_PWM1; 595 | sConfigOC.Pulse = 0; 596 | sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; 597 | sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; 598 | if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) 599 | { 600 | Error_Handler(); 601 | } 602 | if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) 603 | { 604 | Error_Handler(); 605 | } 606 | if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) 607 | { 608 | Error_Handler(); 609 | } 610 | if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) 611 | { 612 | Error_Handler(); 613 | } 614 | /* USER CODE BEGIN TIM5_Init 2 */ 615 | 616 | /* USER CODE END TIM5_Init 2 */ 617 | HAL_TIM_MspPostInit(&htim5); 618 | 619 | } 620 | 621 | /** 622 | * @brief TIM7 Initialization Function 623 | * @param None 624 | * @retval None 625 | */ 626 | static void MX_TIM7_Init(void) 627 | { 628 | 629 | /* USER CODE BEGIN TIM7_Init 0 */ 630 | 631 | /* USER CODE END TIM7_Init 0 */ 632 | 633 | LL_TIM_InitTypeDef TIM_InitStruct = {0}; 634 | 635 | /* Peripheral clock enable */ 636 | LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM7); 637 | 638 | /* TIM7 interrupt Init */ 639 | NVIC_SetPriority(TIM7_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0)); 640 | NVIC_EnableIRQ(TIM7_IRQn); 641 | 642 | /* USER CODE BEGIN TIM7_Init 1 */ 643 | 644 | /* USER CODE END TIM7_Init 1 */ 645 | TIM_InitStruct.Prescaler = 41999; 646 | TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; 647 | TIM_InitStruct.Autoreload = 1; 648 | LL_TIM_Init(TIM7, &TIM_InitStruct); 649 | LL_TIM_EnableARRPreload(TIM7); 650 | LL_TIM_SetTriggerOutput(TIM7, LL_TIM_TRGO_RESET); 651 | LL_TIM_DisableMasterSlaveMode(TIM7); 652 | /* USER CODE BEGIN TIM7_Init 2 */ 653 | 654 | /* USER CODE END TIM7_Init 2 */ 655 | 656 | } 657 | 658 | /** 659 | * @brief UART4 Initialization Function 660 | * @param None 661 | * @retval None 662 | */ 663 | static void MX_UART4_Init(void) 664 | { 665 | 666 | /* USER CODE BEGIN UART4_Init 0 */ 667 | 668 | /* USER CODE END UART4_Init 0 */ 669 | 670 | /* USER CODE BEGIN UART4_Init 1 */ 671 | 672 | /* USER CODE END UART4_Init 1 */ 673 | huart4.Instance = UART4; 674 | huart4.Init.BaudRate = 115200; 675 | huart4.Init.WordLength = UART_WORDLENGTH_8B; 676 | huart4.Init.StopBits = UART_STOPBITS_1; 677 | huart4.Init.Parity = UART_PARITY_NONE; 678 | huart4.Init.Mode = UART_MODE_TX_RX; 679 | huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE; 680 | huart4.Init.OverSampling = UART_OVERSAMPLING_16; 681 | if (HAL_UART_Init(&huart4) != HAL_OK) 682 | { 683 | Error_Handler(); 684 | } 685 | /* USER CODE BEGIN UART4_Init 2 */ 686 | 687 | /* USER CODE END UART4_Init 2 */ 688 | 689 | } 690 | 691 | /** 692 | * @brief UART5 Initialization Function 693 | * @param None 694 | * @retval None 695 | */ 696 | static void MX_UART5_Init(void) 697 | { 698 | 699 | /* USER CODE BEGIN UART5_Init 0 */ 700 | 701 | /* USER CODE END UART5_Init 0 */ 702 | 703 | LL_USART_InitTypeDef USART_InitStruct = {0}; 704 | 705 | LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; 706 | 707 | /* Peripheral clock enable */ 708 | LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_UART5); 709 | 710 | LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC); 711 | LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOD); 712 | /**UART5 GPIO Configuration 713 | PC12 ------> UART5_TX 714 | PD2 ------> UART5_RX 715 | */ 716 | GPIO_InitStruct.Pin = LL_GPIO_PIN_12; 717 | GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; 718 | GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH; 719 | GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; 720 | GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; 721 | GPIO_InitStruct.Alternate = LL_GPIO_AF_8; 722 | LL_GPIO_Init(GPIOC, &GPIO_InitStruct); 723 | 724 | GPIO_InitStruct.Pin = LL_GPIO_PIN_2; 725 | GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; 726 | GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH; 727 | GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL; 728 | GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; 729 | GPIO_InitStruct.Alternate = LL_GPIO_AF_8; 730 | LL_GPIO_Init(GPIOD, &GPIO_InitStruct); 731 | 732 | /* UART5 interrupt Init */ 733 | NVIC_SetPriority(UART5_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0)); 734 | NVIC_EnableIRQ(UART5_IRQn); 735 | 736 | /* USER CODE BEGIN UART5_Init 1 */ 737 | 738 | /* USER CODE END UART5_Init 1 */ 739 | USART_InitStruct.BaudRate = 115200; 740 | USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_8B; 741 | USART_InitStruct.StopBits = LL_USART_STOPBITS_1; 742 | USART_InitStruct.Parity = LL_USART_PARITY_NONE; 743 | USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX; 744 | USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE; 745 | USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16; 746 | LL_USART_Init(UART5, &USART_InitStruct); 747 | LL_USART_ConfigAsyncMode(UART5); 748 | LL_USART_Enable(UART5); 749 | /* USER CODE BEGIN UART5_Init 2 */ 750 | 751 | /* USER CODE END UART5_Init 2 */ 752 | 753 | } 754 | 755 | /** 756 | * @brief USART1 Initialization Function 757 | * @param None 758 | * @retval None 759 | */ 760 | static void MX_USART1_UART_Init(void) 761 | { 762 | 763 | /* USER CODE BEGIN USART1_Init 0 */ 764 | 765 | /* USER CODE END USART1_Init 0 */ 766 | 767 | /* USER CODE BEGIN USART1_Init 1 */ 768 | 769 | /* USER CODE END USART1_Init 1 */ 770 | huart1.Instance = USART1; 771 | huart1.Init.BaudRate = 115200; 772 | huart1.Init.WordLength = UART_WORDLENGTH_8B; 773 | huart1.Init.StopBits = UART_STOPBITS_1; 774 | huart1.Init.Parity = UART_PARITY_NONE; 775 | huart1.Init.Mode = UART_MODE_TX_RX; 776 | huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; 777 | huart1.Init.OverSampling = UART_OVERSAMPLING_16; 778 | if (HAL_UART_Init(&huart1) != HAL_OK) 779 | { 780 | Error_Handler(); 781 | } 782 | /* USER CODE BEGIN USART1_Init 2 */ 783 | 784 | /* USER CODE END USART1_Init 2 */ 785 | 786 | } 787 | 788 | /** 789 | * @brief USART3 Initialization Function 790 | * @param None 791 | * @retval None 792 | */ 793 | static void MX_USART3_UART_Init(void) 794 | { 795 | 796 | /* USER CODE BEGIN USART3_Init 0 */ 797 | 798 | /* USER CODE END USART3_Init 0 */ 799 | 800 | /* USER CODE BEGIN USART3_Init 1 */ 801 | 802 | /* USER CODE END USART3_Init 1 */ 803 | huart3.Instance = USART3; 804 | huart3.Init.BaudRate = 115200; 805 | huart3.Init.WordLength = UART_WORDLENGTH_8B; 806 | huart3.Init.StopBits = UART_STOPBITS_1; 807 | huart3.Init.Parity = UART_PARITY_NONE; 808 | huart3.Init.Mode = UART_MODE_TX_RX; 809 | huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE; 810 | huart3.Init.OverSampling = UART_OVERSAMPLING_16; 811 | if (HAL_UART_Init(&huart3) != HAL_OK) 812 | { 813 | Error_Handler(); 814 | } 815 | /* USER CODE BEGIN USART3_Init 2 */ 816 | 817 | /* USER CODE END USART3_Init 2 */ 818 | 819 | } 820 | 821 | /** 822 | * @brief USART6 Initialization Function 823 | * @param None 824 | * @retval None 825 | */ 826 | static void MX_USART6_UART_Init(void) 827 | { 828 | 829 | /* USER CODE BEGIN USART6_Init 0 */ 830 | 831 | /* USER CODE END USART6_Init 0 */ 832 | 833 | /* USER CODE BEGIN USART6_Init 1 */ 834 | 835 | /* USER CODE END USART6_Init 1 */ 836 | huart6.Instance = USART6; 837 | huart6.Init.BaudRate = 115200; 838 | huart6.Init.WordLength = UART_WORDLENGTH_8B; 839 | huart6.Init.StopBits = UART_STOPBITS_1; 840 | huart6.Init.Parity = UART_PARITY_NONE; 841 | huart6.Init.Mode = UART_MODE_TX_RX; 842 | huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE; 843 | huart6.Init.OverSampling = UART_OVERSAMPLING_16; 844 | if (HAL_UART_Init(&huart6) != HAL_OK) 845 | { 846 | Error_Handler(); 847 | } 848 | /* USER CODE BEGIN USART6_Init 2 */ 849 | 850 | /* USER CODE END USART6_Init 2 */ 851 | 852 | } 853 | 854 | /** 855 | * Enable DMA controller clock 856 | */ 857 | static void MX_DMA_Init(void) 858 | { 859 | 860 | /* DMA controller clock enable */ 861 | __HAL_RCC_DMA1_CLK_ENABLE(); 862 | 863 | /* DMA interrupt init */ 864 | /* DMA1_Stream0_IRQn interrupt configuration */ 865 | HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0); 866 | HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn); 867 | /* DMA1_Stream1_IRQn interrupt configuration */ 868 | HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0); 869 | HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); 870 | /* DMA1_Stream3_IRQn interrupt configuration */ 871 | HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 0, 0); 872 | HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn); 873 | 874 | } 875 | 876 | /** 877 | * @brief GPIO Initialization Function 878 | * @param None 879 | * @retval None 880 | */ 881 | static void MX_GPIO_Init(void) 882 | { 883 | GPIO_InitTypeDef GPIO_InitStruct = {0}; 884 | 885 | /* GPIO Ports Clock Enable */ 886 | __HAL_RCC_GPIOH_CLK_ENABLE(); 887 | __HAL_RCC_GPIOA_CLK_ENABLE(); 888 | __HAL_RCC_GPIOB_CLK_ENABLE(); 889 | __HAL_RCC_GPIOC_CLK_ENABLE(); 890 | __HAL_RCC_GPIOD_CLK_ENABLE(); 891 | 892 | /*Configure GPIO pin Output Level */ 893 | HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8|FLASH_CS_Pin, GPIO_PIN_RESET); 894 | 895 | /*Configure GPIO pins : PA8 FLASH_CS_Pin */ 896 | GPIO_InitStruct.Pin = GPIO_PIN_8|FLASH_CS_Pin; 897 | GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; 898 | GPIO_InitStruct.Pull = GPIO_NOPULL; 899 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; 900 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 901 | 902 | } 903 | 904 | /* USER CODE BEGIN 4 */ 905 | void Encode_Msg_Motor(unsigned char* telemetry_tx_buf) 906 | { 907 | telemetry_tx_buf[0] = 0x88; 908 | telemetry_tx_buf[1] = 0x18; 909 | 910 | telemetry_tx_buf[2] = ccr1 >> 24; 911 | telemetry_tx_buf[3] = ccr1 >> 16; 912 | telemetry_tx_buf[4] = ccr1 >> 8; 913 | telemetry_tx_buf[5] = ccr1; 914 | 915 | telemetry_tx_buf[6] = ccr2 >> 24; 916 | telemetry_tx_buf[7] = ccr2 >> 16; 917 | telemetry_tx_buf[8] = ccr2 >> 8; 918 | telemetry_tx_buf[9] = ccr2; 919 | 920 | telemetry_tx_buf[10] = ccr3 >> 24; 921 | telemetry_tx_buf[11] = ccr3 >> 16; 922 | telemetry_tx_buf[12] = ccr3 >> 8; 923 | telemetry_tx_buf[13] = ccr3; 924 | 925 | telemetry_tx_buf[14] = ccr4 >> 24; 926 | telemetry_tx_buf[15] = ccr4 >> 16; 927 | telemetry_tx_buf[16] = ccr4 >> 8; 928 | telemetry_tx_buf[17] = ccr4; 929 | 930 | telemetry_tx_buf[18] = iBus.LV >> 8; 931 | telemetry_tx_buf[19] = iBus.LV; 932 | // 933 | // telemetry_tx_buf[20] = ((int)pitch.in.pid_result) >> 24; 934 | // telemetry_tx_buf[21] = ((int)pitch.in.pid_result) >> 16; 935 | // telemetry_tx_buf[22] = ((int)pitch.in.pid_result) >> 8; 936 | // telemetry_tx_buf[23] = ((int)pitch.in.pid_result); 937 | // 938 | // telemetry_tx_buf[24] = ((int)roll.in.pid_result) >> 24; 939 | // telemetry_tx_buf[25] = ((int)roll.in.pid_result) >> 16; 940 | // telemetry_tx_buf[26] = ((int)roll.in.pid_result) >> 8; 941 | // telemetry_tx_buf[27] = ((int)roll.in.pid_result); 942 | // 943 | // telemetry_tx_buf[28] = ((int)yaw_heading.in.pid_result) >> 24; 944 | // telemetry_tx_buf[29] = ((int)yaw_heading.in.pid_result) >> 16; 945 | // telemetry_tx_buf[30] = ((int)yaw_heading.in.pid_result) >> 8; 946 | // telemetry_tx_buf[31] = ((int)yaw_heading.in.pid_result); 947 | 948 | // telemetry_tx_buf[32] = ((int)altitude_filt) >> 24; 949 | // telemetry_tx_buf[33] = ((int)altitude_filt) >> 16; 950 | // telemetry_tx_buf[34] = ((int)altitude_filt) >> 8; 951 | // telemetry_tx_buf[35] = ((int)altitude_filt); 952 | 953 | // telemetry_tx_buf[32] = ((int)actual_pressure_fast) >> 24; 954 | // telemetry_tx_buf[33] = ((int)actual_pressure_fast) >> 16; 955 | // telemetry_tx_buf[34] = ((int)actual_pressure_fast) >> 8; 956 | // telemetry_tx_buf[35] = ((int)actual_pressure_fast); 957 | } 958 | 959 | void assign_int_buf(unsigned char* buf, int start, int var){ 960 | for(int i = 0 ; i < 4; i++){ 961 | buf[start + i] = var >>(24-8*i); 962 | } 963 | } 964 | 965 | int Is_iBus_Received(void) 966 | { 967 | if(ibus_rx_cplt_flag==1) 968 | { 969 | ibus_rx_cplt_flag=0; 970 | if(iBus_Check_CHKSUM(&ibus_rx_buf[0],32)==1) 971 | { 972 | iBus_Parsing(&ibus_rx_buf[0], &iBus); 973 | return 1; 974 | } 975 | } 976 | return 0; 977 | } 978 | 979 | void ESC_Calibration(void) 980 | { 981 | TIM5->CCR1 = 167999; 982 | TIM5->CCR2 = 167999; 983 | TIM5->CCR3 = 167999; 984 | TIM5->CCR4 = 167999; 985 | HAL_Delay(7000); 986 | 987 | TIM5->CCR1 = 84000; 988 | TIM5->CCR2 = 84000; 989 | TIM5->CCR3 = 84000; 990 | TIM5->CCR4 = 84000; 991 | HAL_Delay(8000); 992 | } 993 | 994 | void HAL_I2C_MemRxCpltCallback (I2C_HandleTypeDef * hi2c) 995 | { 996 | if(hi2c->Instance == I2C1) 997 | { 998 | mpu_read_busy = 0; 999 | } 1000 | } 1001 | /* USER CODE END 4 */ 1002 | 1003 | /** 1004 | * @brief This function is executed in case of error occurrence. 1005 | * @retval None 1006 | */ 1007 | void Error_Handler(void) 1008 | { 1009 | /* USER CODE BEGIN Error_Handler_Debug */ 1010 | /* User can add his own implementation to report the HAL error return state */ 1011 | __disable_irq(); 1012 | while (1) 1013 | { 1014 | } 1015 | /* USER CODE END Error_Handler_Debug */ 1016 | } 1017 | 1018 | #ifdef USE_FULL_ASSERT 1019 | /** 1020 | * @brief Reports the name of the source file and the source line number 1021 | * where the assert_param error has occurred. 1022 | * @param file: pointer to the source file name 1023 | * @param line: assert_param error line source number 1024 | * @retval None 1025 | */ 1026 | void assert_failed(uint8_t *file, uint32_t line) 1027 | { 1028 | /* USER CODE BEGIN 6 */ 1029 | /* User can add his own implementation to report the file name and line number, 1030 | ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ 1031 | /* USER CODE END 6 */ 1032 | } 1033 | #endif /* USE_FULL_ASSERT */ 1034 | 1035 | --------------------------------------------------------------------------------