Here is my code:
#include "main.h"
#include "i2c.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "LoRa.h"
#include "stdio.h"
#include "string.h"
#include "MATH.h"
#include "ServoBLDC.h"
#include "BME280_STM32.h"
#include "mpu6050.h"
//#include "UartRingbuffer.h"
//#include "NMEA.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define SEND_DATA 0xEE
#define RECV_DATA 0xFF
#define Calibrate 0 //To calibrate the ESC set it to 1
#define SERVO1 0
#define SERVO2 1
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
LoRa myLoRa;
uint8_t NewData = 0;
uint8_t RX_BUF[25];
uint8_t TX_BUF[12];
uint8_t RX_CNT = 0;
uint8_t TX_CNT = 0;
uint8_t REC_DATA_SIZE;
uint16_t LoRa_State = 0;
uint16_t Data[10];
uint16_t pulseValue = 0;
uint16_t PWM_res = 0;
float temp = 0.0;
uint16_t min_pulse_width = 0; //650ms
uint16_t max_pulse_width = 0; //2000ms
long Angle1;
long Angle2;
MPU6050_t mpu;
Kalman_t kalman;
int mpu_ok;
int bmp_ok;
double GX, GY, GZ, AX, AY, AZ;
double ROLL, PITCH, M_ROLL, M_PITCH;
float gex, gey, gez, aex, aey;
float Roll, Pitch, Yaw, M_Roll, M_Pitch, M_Yaw;
float Temperature, Pressure, Humidity, Altitude;
int temperature, pressure, humidity, altitude;
char GGA[100];
char RMC[100];
//GPSSTRUCT gpsData;
int flagGGA = 0, flagRMC = 0;
char lcdBuffer [50];
int VCCTimeout = 5000; // GGA or RMC will not be received if the VCC is not sufficient
uint16_t size;
char Data_T[16];
char Data_P[16];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_SPI1_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
myLoRa = newLoRa();
myLoRa.CS_port = NSS_GPIO_Port;
myLoRa.CS_pin = NSS_Pin;
myLoRa.reset_port = RST_GPIO_Port;
myLoRa.reset_pin = RST_Pin;
myLoRa.DIO0_port = DIO0_GPIO_Port;
myLoRa.DIO0_pin = DIO0_Pin;
myLoRa.hSPIx = &hspi1;
myLoRa.frequency = 433;
myLoRa.spredingFactor = SF_7;
myLoRa.bandWidth = BW_125KHz;
myLoRa.crcRate = CR_4_5;
myLoRa.power = POWER_20db;
myLoRa.overCurrentProtection = 100;
myLoRa.preamble = 8;
if(LoRa_init(&myLoRa) == LORA_OK)
LoRa_State = 1;
else
LoRa_State = 0;
LoRa_startReceiving(&myLoRa);
#if Calibrate
TIM1->CCR1 = 1000; //Set the maximum pulse (2ms)
HAL_Delay(2000); //Wait for 1 beep
TIM1->CCR1 = 500; //Set the minimum pulse (1ms)
HAL_Delay(1000); //Wait for 2 beep
TIM1->CCR1 = 0; //Reset to 0, so it can be controlled via ADC
#endif
motorInit(SERVO1);
motorInit(SERVO2);
BME280_Config(OSRS_2, OSRS_16, OSRS_1, MODE_NORMAL, T_SB_0p5, IIR_16);
/* Ringbuf_init();
HAL_Delay(500);
*/
if(MPU6050_Init(&hi2c1) == 1)
{
mpu_ok = 0;
}
else
{
mpu_ok = 1;
}
calculate_IMU_error(&hi2c1, &mpu);
// MPU_calibrateGyro(&hi2c1, &mpu, 1000);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
if(NewData)
{
REC_DATA_SIZE = LoRa_receive(&myLoRa, RX_BUF, sizeof(RX_BUF));
NewData = 0;
if(RX_BUF[22] == SEND_DATA)
{
if(LoRa_transmit(&myLoRa, TX_BUF, sizeof(TX_BUF), 500))
{
TX_CNT = (TX_CNT + 1) % 100;
TX_BUF[0] = TX_CNT;
TX_BUF[2] = (int)(altitude / 100); //First 2 digits
TX_BUF[3] = (int)(altitude % 100); //Last 2 digits
TX_BUF[4] = (int)(pressure / 10000); //First 2 digits
TX_BUF[5] = (int)(pressure / 100) % 100; //Middle 2 digits
TX_BUF[6] = (int)(pressure % 100); //Last 2 digits
TX_BUF[7] = (int)(temperature / 10);
TX_BUF[8] = (int)(temperature % 10);
TX_BUF[9] = (int)(humidity / 100);
TX_BUF[10] = (int)(humidity % 100);
}
}
else if(RX_BUF[22] == RECV_DATA)
{
RX_CNT = (RX_CNT + 1) % 100;
HAL_GPIO_TogglePin(GPIOB, LED_REC_Pin);
}
}
for(int i = 0; i < 10;i += 2){
Data[i/2] = RX_BUF[i] | RX_BUF[i+1] << 8;
}
BME280_Measure();
BME280_Altitude();
altitude = (int)(Altitude * 10);
pressure = (int)(Pressure * 10);
temperature = (int)(Temperature * 10);
humidity = (int)(Humidity * 10);
MPU6050_Read_Gyro(&hi2c1, &mpu);
MPU6050_Read_Accel(&hi2c1, &mpu);
// RollPitchYaw_CompFilter(&hi2c1, &mpu);
//
//// MPU_calcAttitude(&hi2c1, &mpu);
//
// Roll = mpu.roll;
// Pitch = mpu.pitch;
// Yaw = mpu.yaw;
//
// MPU6050_Read_All(&hi2c1, &mpu);
//
ROLL = mpu.KalmanAngleX;
PITCH = mpu.KalmanAngleY;
//
// M_ROLL = map(ROLL, -90, 90, 500, 1000);
// M_PITCH = map(PITCH, -90, 90, 500, 1000);
/* Angle1 = map(Data[3], 0, 4095, 500, 1000);
moveToangle(SERVO1, Angle1);
Angle2 = map(Data[4], 0, 4095, 500, 1000);
moveToangle(SERVO2, Angle2);
*/
}
/* USER CODE END 3 */
}
