C++ (Cpp) Get_Gyro_Rates примеры использования

Пример #1

Файл: MPU6050.cpp Проект: SeniorDesign2014/MainFirmware

/********************************************************************
This is the primary implementation for the motion sensor. It checks the
gyroscope and accelerometer for changing values. If the values have changed
beyond the starting threshold, it will return ASCII 1.
********************************************************************/
char motion_update(){
	float accelXdiff;
	float accelYdiff;
	float accelZdiff;
	float gyroXdiff;
	float gyroYdiff;
	float gyroZdiff;
	
	//check accelerometer against initial values
	Get_Accel_Values();
	Get_Accel_Angles();
	accelXdiff = ACCEL_XANGLE - ACCEL_XOUT_ARMED;
	accelYdiff = ACCEL_YANGLE - ACCEL_YOUT_ARMED;
	//TODO: fix
	//accelZdiff = ACCEL_ZANGLE - ACCEL_ZANGLE_ARMED;
	
	if((fabsf(accelXdiff) > ACCEL_X_TOLERANCE)|| (fabsf(accelYdiff) > ACCEL_Y_TOLERANCE)){ 
		//	|| (fabsf(accelZdiff) > ACCEL_Z_TOLERANCE)){
		return('1'); 	//something accelerometery is going on
	}
	
	//check gyroscope against initial values
	Get_Gyro_Rates();
	gyroXdiff = GYRO_XOUT - GYRO_XOUT_ARMED;
	gyroYdiff = GYRO_YOUT - GYRO_YOUT_ARMED;
	gyroZdiff = GYRO_ZOUT - GYRO_ZOUT_ARMED;
	
	if((fabsf(gyroXdiff) > GYRO_X_TOLERANCE)|| (fabsf(gyroYdiff) > GYRO_Y_TOLERANCE) || (fabsf(gyroZdiff) > GYRO_Z_TOLERANCE)){
		return('1');	//something gyroscopey is going on
	}
	
	return('0');	//nothing fishy is going on
}

Пример #2

Файл: MPU6050.cpp Проект: SeniorDesign2014/MainFirmware

void motion_arm_position(void){
	Get_Gyro_Rates();
	GYRO_XOUT_ARMED = GYRO_XOUT;
	GYRO_YOUT_ARMED = GYRO_YOUT;
	GYRO_ZOUT_ARMED = GYRO_ZOUT;
	
	Get_Accel_Values();
	Get_Accel_Angles();
	
	ACCEL_XOUT_ARMED = ACCEL_XANGLE;
	ACCEL_YOUT_ARMED = ACCEL_YANGLE;
	ACCEL_ZOUT_ARMED = ACCEL_ZANGLE;
}

Пример #3

Файл: Timer1.c Проект: Jean-Emile/Quadrocopter

void _ISR _T1Interrupt(void)
{	
	IFS0bits.T1IF = 0; //Clear interrupt flag
	IEC0bits.T1IE = 0; //Disable timer1 interrupt
	LATAbits.LATA0 = !LATAbits.LATA0;
	
	Get_Gyro_Rates();
	Get_Accel_Values();
	Get_Accel_Angles();	
	
	//complementary_filter();
	second_order_complementary_filter();
	
	update_PID();
	update_motors_single_shot();
	IEC0bits.T1IE = 1; //Enable timer1 interrupt
}

Пример #4

Файл: main.c Проект: LChiara/DuctedFan2016

void Callback_20ms() {
	desiredState.key.abs.pos.z = altitudeValue / 1000.0;

	/* Setting motor speed based on altitude */
	lcd_buffer_print(LCD_LINE4, "In: %1.3f", sonarDistance);
	outValue = PID_Compute(sonarDistance, desiredState.key.abs.pos.z, &z_axis_PID);
	desiredState.key.avg_motor_us = map(outValue * map(analogRead, 0, 4096, 0, 1), 0, 0.5, MOTOR_MIN_US, MOTOR_MAX_US);
	lcd_buffer_print(LCD_LINE5, "Mot: %4.0f", desiredState.key.avg_motor_us);
	//lcd_buffer_print(LCD_LINE7, "Diff: %3.0f", desiredState.key.motor_diff_us);

	/* Obtain accelerometer an gyro values */
	Get_Gyro_Rates(&currentState.key.gyro.vel.x, &currentState.key.gyro.vel.y, &currentState.key.gyro.vel.z);
	Get_Accel_Angles(&currentState.key.accel.pos.x, &currentState.key.accel.pos.y);
	//Get_Mag_Value_Normalized(&currentState.key.magn.pos.x, &currentState.key.magn.pos.y, &currentState.key.magn.pos.z);

	Get_Accel_Gravity_Power(&currentState.key.accel.vel.x, &currentState.key.accel.vel.y, &currentState.key.accel.vel.z);
	get_Angle_AHRS(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, &currentState.key.Kalman.acc.x, &currentState.key.Kalman.acc.y, &currentState.key.Kalman.acc.z);
	// get_Angle_AHRS_Mahony(currentState.key.gyro.vel.x, currentState.key.gyro.vel.y, currentState.key.gyro.vel.z, currentState.key.accel.vel.x, currentState.key.accel.vel.y, currentState.key.accel.vel.z, currentState.key.magn.pos.x, currentState.key.magn.pos.y, currentState.key.magn.pos.z, &currentState.key.Kalman.vel.x, &currentState.key.Kalman.vel.y, &currentState.key.Kalman.vel.z);

	/* Calcolo Roll e Pitch con il filtro di Kalman */
	 currentState.key.Kalman.pos.x = Compute_AVG(getAngle(currentState.key.accel.pos.x, currentState.key.gyro.vel.x, dt, rollKalman), &rollAVG);
	 currentState.key.Kalman.pos.y = Compute_AVG(getAngle(currentState.key.accel.pos.y, currentState.key.gyro.vel.y, dt, pitchKalman), &pitchAVG);
	//currentState.key.Kalman.pos.z = currentState.key.Kalman.acc.z;

	// Angolo di Yaw mediante giroscopio
	// currentState.key.Kalman.pos.z = currentState.key.gyro.vel.z;

	// Get_Mag_Heading_Compensated (&currentState.key.Kalman.pos.z, currentState.key.Kalman.pos.x, currentState.key.Kalman.pos.y);
	// Get_Mag_Heading(&currentState.key.Kalman.acc.z);


	 desiredState.key.x_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.x, 0, &Roll_PID), -30, 30, 60, 120);
	 desiredState.key.y_servo_deg = map(PID_Compute(-currentState.key.Kalman.pos.y, 0, &Pitch_PID), -30, 30, 60, 120);

	 currentState.key.Kalman.pos.z += currentState.key.gyro.vel.z*dt;


	Motor_Write_us(MOTOR_UPPER, desiredState.key.avg_motor_us + desiredState.key.motor_diff_us);
	Motor_Write_us(MOTOR_BOTTOM, desiredState.key.avg_motor_us - desiredState.key.motor_diff_us);

	Servo_Write_deg(SERVO_ROLL, desiredState.key.x_servo_deg);
	Servo_Write_deg(SERVO_PITCH, desiredState.key.y_servo_deg); // pitch servo is trimmed 18°
}

Пример #5

Файл: main.c Проект: jiaxinguo/School-Work

/**************************************************************************//**
 * @brief  Main function
 *****************************************************************************/
int main(void)
{
  /* Setup SysTick Timer for 10 msec interrupts  */
  if (SysTick_Config(CMU_ClockFreqGet(cmuClock_CORE) / 1000))
  {
    while (1) ;
  }

  /* Initialize the display module. */
  DISPLAY_Init();

  /* Retarget stdio to a text display. */
  if (RETARGET_TextDisplayInit() != TEXTDISPLAY_EMSTATUS_OK)
  {
    while (1) ;
  }

  /* Output text on Memory LCD */
  printf("Hello, EFM32 Zero Gecko world!");
  Delay(2000);

  /* Clear screen */
  printf("\f");

  Setup_MPU6050();
  MPU6050_Test_I2C();
  MPU6050_Check_Registers();
  Calibrate_Gyros();
  Calibrate_Acc();
  /* Update Memory LCD display forever */
  while (1)
  {
	Get_Gyro_Rates();
	Get_Accel_Values();

	Delay(500);
	printf("\f");
  }
}