FAQs
cancel
Turn on suggestions
Showing results for 
Search instead for 
Did you mean: 

Pitch, yaw and roll

hamo
Senior

‎2024-02-06 12:52 AM

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.

 

Labels:
0 Kudos
4 REPLIES 4
Andrew Neil
Guru

‎2024-02-06 1:38 AM

@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:

AndrewNeil_0-1707212188468.png

 

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

AndrewNeil_1-1707212188457.png

 

 

hamo
Senior

‎2024-02-06 2:02 AM

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.

0 Kudos
Andrew Neil
Guru
In response to hamo

‎2024-02-06 2:28 AM

@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.

0 Kudos
hamo
Senior

‎2024-02-06 10:08 PM

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 */
}
View more
0 Kudos
Top