aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor_MPU6000: replaced _cs_pin parameter with #define and saved 1 byte of memory

Browse Source 
Updated ArduCopter, ArduPlane and example sketches in AP_InertialSensor, AP_IMU and AP_AHRS libraries because they no longer need to pass in cs_pin to the constructor
This commit is contained in:
rmackay9 2012-09-28 19:21:59 +09:00
parent 2fd9022f03
commit 4bca609b9f
9 changed files with 28 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -256,7 +256,7 @@ AP_GPS_None g_gps_driver(NULL);
#endif // GPS PROTOCOL
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
AP_InertialSensor_MPU6000 ins;
#else
AP_InertialSensor_Oilpan ins(&adc);
#endif

6
ArduCopter/config.h
View File

@ -92,12 +92,6 @@
# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
# ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
# endif
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
# define NUM_IMU_SAMPLES_FOR_200HZ 5
# define NUM_IMU_SAMPLES_FOR_100HZ 10

2
ArduPlane/ArduPlane.pde
View File

@ -214,7 +214,7 @@ AP_GPS_None g_gps_driver(NULL);
#endif // GPS PROTOCOL
# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
AP_InertialSensor_MPU6000 ins;
# else
AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE

6
ArduPlane/config.h
View File

@ -146,12 +146,6 @@
# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN
#endif
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
# ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// ADC Enable - used to eliminate for systems which don't have ADC.
//

2
libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde
View File

@ -44,7 +44,7 @@ AP_Compass_HMC5843 compass;
#endif
#ifdef APM2_HARDWARE
AP_InertialSensor_MPU6000 ins( 53 );
AP_InertialSensor_MPU6000 ins;
# else
AP_ADC_ADS7844 adc;
AP_InertialSensor_Oilpan ins( &adc );

2
libraries/AP_IMU/examples/IMU_MPU6000_test/IMU_MPU6000_test.pde
View File

@ -18,7 +18,7 @@ FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess mpu_scheduler;
AP_InertialSensor_MPU6000 mpu6000( 53 ); /* chip select is pin 53 */
AP_InertialSensor_MPU6000 mpu6000;
AP_IMU_INS imu(&mpu6000, 0); /* second arg is for Parameters. Can we leave it null?*/
#define A_LED_PIN 27

46
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -161,9 +161,6 @@
#define GYRO_BIAS_FROM_GRAVITY_RATE 4 // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
#define DEFAULT_ACCEL_FUSION_GAIN 0x80 // Default gain for accel fusion (with gyros)
uint8_t AP_InertialSensor_MPU6000::_cs_pin;
/*
* RS-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
@ -204,9 +201,8 @@ uint8_t AP_InertialSensor_MPU6000::_fifoCountL; // low byte of n
Quaternion AP_InertialSensor_MPU6000::quaternion; // holds the 4 quaternions representing attitude taken directly from the DMP
AP_PeriodicProcess* AP_InertialSensor_MPU6000::_scheduler = NULL;
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( uint8_t cs_pin )
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
{
_cs_pin = cs_pin; /* can't use initializer list, is static */
_gyro.x = 0;
_gyro.y = 0;
_gyro.z = 0;
@ -355,13 +351,13 @@ static int16_t spi_transfer_16(void)
void AP_InertialSensor_MPU6000::read(uint32_t)
{
// now read the data
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
byte addr = MPUREG_ACCEL_XOUT_H | 0x80;
SPI.transfer(addr);
for (uint8_t i=0; i<7; i++) {
_sum[i] += spi_transfer_16();
}
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
_count++;
if (_count == 0) {
@ -382,22 +378,22 @@ uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg )
uint8_t return_value;
uint8_t addr = reg | 0x80; // Set most significant bit
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(addr);
return_value = SPI.transfer(0);
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
return return_value;
}
void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
{
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(reg);
SPI.transfer(val);
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
// MPU6000 new data interrupt on INT6
@ -416,8 +412,8 @@ void AP_InertialSensor_MPU6000::data_interrupt(void)
void AP_InertialSensor_MPU6000::hardware_init()
{
// MPU6000 chip select setup
pinMode(_cs_pin, OUTPUT);
digitalWrite(_cs_pin, HIGH);
pinMode(MPU6000_CS_PIN, OUTPUT);
digitalWrite(MPU6000_CS_PIN, HIGH);
delay(1);
// Chip reset
@ -575,12 +571,12 @@ void AP_InertialSensor_MPU6000::dmp_register_write(uint8_t bank, uint8_t address
{
register_write(MPUREG_BANK_SEL,bank);
register_write(MPUREG_MEM_START_ADDR,address);
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
for (uint8_t i=0; i<num_bytes; i++) {
SPI.transfer(data[i]);
}
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
// MPU6000 DMP initialization
@ -695,12 +691,12 @@ void AP_InertialSensor_MPU6000::FIFO_getPacket()
int16_t q_long[4];
uint8_t addr = MPUREG_FIFO_R_W | 0x80; // Set most significant bit to indicate a read
uint8_t received_packet[DMP_FIFO_BUFFER_SIZE]; // FIFO packet buffer
digitalWrite(_cs_pin, LOW); // enable the device
digitalWrite(MPU6000_CS_PIN, LOW); // enable the device
SPI.transfer(addr); // send address we want to read from
for(i = 0; i < _fifoCountL; i++) {
received_packet[i] = SPI.transfer(0); // request value
}
digitalWrite(_cs_pin, HIGH); // disable device
digitalWrite(MPU6000_CS_PIN, HIGH); // disable device
// we are using 16 bits resolution
q_long[0] = (int16_t) ((((uint16_t) received_packet[0]) << 8) + ((uint16_t) received_packet[1]));
@ -887,13 +883,13 @@ void AP_InertialSensor_MPU6000::dmp_set_sensor_fusion_accel_gain(uint8_t gain)
//inv_key_0_96
register_write(MPUREG_BANK_SEL,0x00);
register_write(MPUREG_MEM_START_ADDR, 0x60);
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
SPI.transfer(0x00);
SPI.transfer(gain); // Original : 0x80 To test: 0x40, 0x20 (too less)
SPI.transfer(0x00);
SPI.transfer(0x00);
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
// Load initial memory values into DMP memory banks
@ -905,13 +901,13 @@ void AP_InertialSensor_MPU6000::dmp_load_mem()
for(uint8_t j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
register_write(MPUREG_MEM_START_ADDR,start_addy);
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 16; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[i][j][k]));
SPI.transfer((uint8_t) byteToSend);
}
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
}
@ -919,23 +915,23 @@ void AP_InertialSensor_MPU6000::dmp_load_mem()
for(uint8_t j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
register_write(MPUREG_MEM_START_ADDR,start_addy);
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 16; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[7][j][k]));
SPI.transfer((uint8_t) byteToSend);
}
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
register_write(MPUREG_MEM_START_ADDR,0x80);
digitalWrite(_cs_pin, LOW);
digitalWrite(MPU6000_CS_PIN, LOW);
SPI.transfer(MPUREG_MEM_R_W);
for(int k = 0; k < 9; k++) {
uint8_t byteToSend = pgm_read_byte((const prog_char *)&(dmpMem[7][8][k]));
SPI.transfer((uint8_t) byteToSend);
}
digitalWrite(_cs_pin, HIGH);
digitalWrite(MPU6000_CS_PIN, HIGH);
}
// ========= DMP MEMORY ================================

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -10,6 +10,7 @@
#include "../AP_Math/AP_Math.h"
#include "AP_InertialSensor.h"
#define MPU6000_CS_PIN 53 // APM pin connected to mpu6000's chip select pin
#define DMP_FIFO_BUFFER_SIZE 72 // DMP FIFO buffer size
// DMP memory
@ -19,7 +20,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
{
public:
AP_InertialSensor_MPU6000( uint8_t cs_pin );
AP_InertialSensor_MPU6000();
uint16_t init( AP_PeriodicProcess * scheduler );
static void dmp_init(); // Initialise MPU6000's DMP
@ -82,9 +83,6 @@ private:
static AP_PeriodicProcess* _scheduler; // pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
/* TODO deprecate _cs_pin */
static uint8_t _cs_pin;
// ensure we can't initialise twice
bool _initialised;

2
libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
View File

@ -16,7 +16,7 @@ FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
AP_InertialSensor_MPU6000 ins( 53 ); /* chip select is pin 53 */
AP_InertialSensor_MPU6000 ins;
void setup(void)
{