Roll, Pitch and Yaw rates for ArduPilot

I am using a ArduPilot board with ArduIMU V2+ for pitch roll and yaw sensing. The default code processes roll, pitch and yaw (ground course) and sends it into ArduPilot. ArduPilot thus has RLL, PCH and CRS information reflecting the respective angles in degress.

 

However, I need to get the roll, pitch and yaw rates. Which is the information before ArduIMU passes into DCM to process to become the angles. I'm kinda amateur at programming and am quite clueless about passing the gyro vector (or should I pass corrected gyro data?) data to ArduPilot and thus obtaining the information via wireless telemetry?

 

Is there anywhere I can find code for this? Or any resources that I can refer to write code for this?

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  •  I wrote this few days ago. It works in mine. Be careful, when the program is loading do not move the ardupilot. It needs to calibrate 

  • // Sumit Pokhrel - yaw ,pitch and roll rate .
    //

    #include <stdarg.h>
    #include <AP_Common.h>
    #include <AP_Progmem.h>
    #include <AP_HAL.h>
    #include <AP_HAL_AVR.h>
    #include <AP_HAL_AVR_SITL.h>
    #include <AP_HAL_Empty.h>
    #include <AP_Math.h>
    #include <AP_Param.h>
    #include <AP_ADC.h>
    #include <AP_InertialSensor.h>

    //ArduPilot Hardware Abstraction Layer
    const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;

    //Invensense's 6 DoF Accelerometer/Gyro MPU-6000 chip
    AP_InertialSensor_MPU6000 ins;

    void setup(void)
    {
    // Turn off the barometer from holding the SPI bus, that leads to bus collisions
    hal.gpio->pinMode(40, GPIO_OUTPUT);
    hal.gpio->write(40, 1);

    // Initialize the MPU 6000 sensor - Vehicle must be kept still as gyros will calibrate
    ins.init(AP_InertialSensor::COLD_START,
    AP_InertialSensor::RATE_100HZ,
    NULL);
    hal.console->println("MPU 6000 sensor calibrated and initialized");

    }

    void loop(void)

    {

    ins.update ();

    // Ask MPU6000 for gyro data
    Vector3f gyro = ins.get_gyro();
    float gyropitch = ToDeg(gyro.y), gyroroll = ToDeg(gyro.x), gyroyaw = ToDeg(gyro.z);

    hal.console->printf_P(PSTR("\n Pitch:%4.4f \t Yaw:%4.4f \t Roll:%4.4f "), //4.2 -> 2 decimal digits and 4 widths
    gyropitch, gyroroll, gyroyaw);

    }

    AP_HAL_MAIN();

This reply was deleted.

Activity

Neville Rodrigues liked Neville Rodrigues's profile
Jun 30
Santiago Perez liked Santiago Perez's profile
Jun 21
More…