Pitch, yaw and roll

[hamo]

Hello,

i have used the library i aploaded to calculate roll, pitch and yaw.

I am using MPU6050 with STM32F103C8 and KEIL IDE.

For pitch and roll i get good results but i cann't get the result for yaw.

I use kalman filter for that.

 

[Andrew Neil]

---

@hamo wrote:

the library

---

What library?

You're also going to need to show your code which uses that library.

 

---

@hamo wrote:

i cann't get the result for yaw.

---

What, exactly, does that mean?

• Does it crash?
• Just gives a constant value?
• Gives a random value?
• Has an offset/
• other??

---

@hamo wrote:

I use kalman filter for that.

---

Are you getting good raw data from the sensor before your filtering?

 

Use this button to properly post source code:

 

To get that extra row of icons, press this button:

 

 

[hamo]

Thanks for your replay.
I am using this library:
https://github.com/leech001/MPU6050
My code i will post it in other post because i am not at home now.
For roll and pitch i am getting good data after filtering, i didn't tried to get data before filtering.

For yaw the value just keeps increment.

[Andrew Neil]

---

@hamo wrote:

i didn't tried to get data before filtering.

For yaw the value just keeps increment.

---

You should definitely do that - to know whether your problem is in the filter, or before it.

[hamo]

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 */
}