yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Updated imu.h and imu.ino and increased command argv array size from 16 to 32 to handel PID update

Browse Source
This commit is contained in:
Patrick Goebel 2016-04-11 08:03:37 -07:00
parent 6234b9260a
commit 79e5a3b1a8
3 changed files with 31 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
View File

@ -143,6 +143,7 @@
#ifdef USE_IMU
#include "imu.h"
// The only IMU currently supported is the Adafruit 9-DOF IMU
#define ADAFRUIT_9DOF
#endif
@ -161,8 +162,8 @@ char chr;
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
char argv1[32];
char argv2[32];
// The arguments converted to integers
long arg1;
@ -220,6 +221,9 @@ int runCommand() {
#ifdef USE_IMU
case READ_IMU:
imu_data = readIMU();
/* Send the IMU data base in the following order
* [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch]
*/
Serial.print(imu_data.ax);
Serial.print(F(" "));
Serial.print(imu_data.ay);
@ -242,7 +246,7 @@ int runCommand() {
Serial.print(F(" "));
Serial.print(imu_data.pitch);
Serial.print(F(" "));
Serial.println(imu_data.uh);
Serial.println(imu_data.ch);
break;
#endif
#ifdef USE_SERVOS

12
ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h
View File

@ -3,6 +3,16 @@
#ifndef IMU_H
#define IMU_H
/*
IMU data is assumed to be returned in the following order:
[ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch]
where a stands for accelerometer, g for gyroscope and m for magnetometer.
The last value ch stands for "compensated heading" that some IMU's can
compute to compensate magnetic heading from the current roll and pitch.
*/
typedef struct imuData_s
{
float ax = -999;
@ -16,7 +26,7 @@ typedef struct imuData_s
float mz = -999;
float roll = -999;
float pitch = -999;
float uh = -999;
float ch = -999;
} imuData;
void initIMU();

21
ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
View File

@ -12,22 +12,17 @@
Adafruit_9DOF dof = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
/* Update this with the correct SLP for accurate altitude measurements */
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
void initIMU()
{
if(!accel.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
}
if(!mag.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
}
accel.begin();
mag.begin();
gyro.begin();
}
imuData readIMU() {
@ -36,6 +31,7 @@
sensors_event_t accel_event;
sensors_event_t mag_event;
sensors_vec_t orientation;
sensors_event_t event;
/* Calculate pitch and roll from the raw accelerometer data */
accel.getEvent(&accel_event);
@ -43,6 +39,11 @@
data.ay = accel_event.acceleration.y;
data.az = accel_event.acceleration.z;
gyro.getEvent(&event);
data.gx = event.gyro.x;
data.gy = event.gyro.y;
data.gz = event.gyro.z;
if (dof.accelGetOrientation(&accel_event, &orientation))
{
/* 'orientation' should have valid .roll and .pitch fields */