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?
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();