Skip to content

Commit 0a2db6e

Browse files
Hananja from Raspberry Pi Zero 2 WHananja from Raspberry Pi Zero 2 W
Hananja from Raspberry Pi Zero 2 W
authored and
Hananja from Raspberry Pi Zero 2 W
committed
Modify for Linux
1 parent 69c9f64 commit 0a2db6e

3 files changed

Lines changed: 78 additions & 113 deletions

File tree

‎LinuxI2CDev/MPU6050/MPU6050.cpp‎

Lines changed: 33 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,12 @@ THE SOFTWARE.
3737
*/
3838

3939
#include "MPU6050.h"
40+
#include <math.h>
41+
#include <time.h>
42+
#include <stdlib.h>
43+
#include <stdio.h>
44+
#include <string.h>
45+
#include <unistd.h>
4046

4147
/** Specific address constructor.
4248
* @param address I2C address, uses default I2C address if none is specified
@@ -2762,6 +2768,14 @@ void MPU6050_Base::setFIFOTimeout(uint32_t fifoTimeout) {
27622768
this->fifoTimeout = fifoTimeout;
27632769
}
27642770

2771+
/** Replacement for Arduino micros().
2772+
* @return Number of microseconds since process start
2773+
*/
2774+
static uint32_t micros() {
2775+
struct timespec ts;
2776+
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts);
2777+
return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
2778+
}
27652779
/** Get latest byte from FIFO buffer no matter how much time has passed.
27662780
* === GetCurrentFIFOPacket ===
27672781
* ================================================================
@@ -3118,7 +3132,7 @@ bool MPU6050_Base::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint
31183132

31193133
if (useProgMem) {
31203134
// write the chunk of data as specified
3121-
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
3135+
for (j = 0; j < chunkSize; j++) progBuffer[j] = *(data + i + j);
31223136
} else {
31233137
// write the chunk of data as specified
31243138
progBuffer = (uint8_t *)data + i;
@@ -3188,9 +3202,9 @@ bool MPU6050_Base::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSi
31883202
uint8_t bank, offset, length;
31893203
for (i = 0; i < dataSize;) {
31903204
if (useProgMem) {
3191-
bank = pgm_read_byte(data + i++);
3192-
offset = pgm_read_byte(data + i++);
3193-
length = pgm_read_byte(data + i++);
3205+
bank = *(data + i++);
3206+
offset = *(data + i++);
3207+
length = *(data + i++);
31943208
} else {
31953209
bank = data[i++];
31963210
offset = data[i++];
@@ -3208,7 +3222,7 @@ bool MPU6050_Base::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSi
32083222
Serial.println(length);*/
32093223
if (useProgMem) {
32103224
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
3211-
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
3225+
for (j = 0; j < length; j++) progBuffer[j] = *(data + i + j);
32123226
} else {
32133227
progBuffer = (uint8_t *)data + i;
32143228
}
@@ -3221,7 +3235,7 @@ bool MPU6050_Base::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSi
32213235
// behavior only, and exactly why (or even whether) it has to be here
32223236
// is anybody's guess for now.
32233237
if (useProgMem) {
3224-
special = pgm_read_byte(data + i++);
3238+
special = *(data + i++);
32253239
} else {
32263240
special = data[i++];
32273241
}
@@ -3275,7 +3289,12 @@ void MPU6050_Base::setDMPConfig2(uint8_t config) {
32753289
I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config, wireObj);
32763290
}
32773291

3278-
3292+
/* Replacement for Arduino map()
3293+
* @see https://www.arduino.cc/reference/en/language/functions/math/map/
3294+
*/
3295+
long map(long x, long in_min, long in_max, long out_min, long out_max) {
3296+
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
3297+
}
32793298
//***************************************************************************************
32803299
//********************** Calibration Routines **********************
32813300
//***************************************************************************************
@@ -3319,7 +3338,6 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
33193338
uint32_t eSum;
33203339
uint16_t gravity = 8192; // prevent uninitialized compiler warning
33213340
if (ReadAddress == 0x3B) gravity = 16384 >> getFullScaleAccelRange();
3322-
Serial.write('>');
33233341
for (int i = 0; i < 3; i++) {
33243342
I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word)
33253343
Reading = Data;
@@ -3350,13 +3368,11 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
33503368
}
33513369
if((c == 99) && eSum > 1000){ // Error is still to great to continue
33523370
c = 0;
3353-
Serial.write('*');
33543371
}
33553372
if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance
33563373
if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop
3357-
delay(1);
3374+
usleep(1000);
33583375
}
3359-
Serial.write('.');
33603376
kP *= .75;
33613377
kI *= .75;
33623378
for (int i = 0; i < 3; i++){
@@ -3386,12 +3402,12 @@ int16_t * MPU6050_Base::GetActiveOffsets() {
33863402
void MPU6050_Base::PrintActiveOffsets() {
33873403
GetActiveOffsets();
33883404
// A_OFFSET_H_READ_A_OFFS(Data);
3389-
Serial.print((float)offsets[0], 5); Serial.print(",\t");
3390-
Serial.print((float)offsets[1], 5); Serial.print(",\t");
3391-
Serial.print((float)offsets[2], 5); Serial.print(",\t");
3405+
fprintf(stderr, "%.5f,\t", (float)offsets[0]);
3406+
fprintf(stderr, "%.5f,\t", (float)offsets[1]);
3407+
fprintf(stderr, "%.5f,\t", (float)offsets[2]);
33923408

33933409
// XG_OFFSET_H_READ_OFFS_USR(Data);
3394-
Serial.print((float)offsets[3], 5); Serial.print(",\t");
3395-
Serial.print((float)offsets[4], 5); Serial.print(",\t");
3396-
Serial.print((float)offsets[5], 5); Serial.print("\n\n");
3410+
fprintf(stderr, "%.5f,\t", (float)offsets[3]);
3411+
fprintf(stderr, "%.5f,\t", (float)offsets[4]);
3412+
fprintf(stderr, "%.5f\n\n", (float)offsets[5]);
33973413
}

‎LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.cpp‎

Lines changed: 42 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -36,53 +36,10 @@ THE SOFTWARE.
3636
#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
3737

3838
#include "MPU6050_6Axis_MotionApps20.h"
39-
40-
// Tom Carpenter's conditional PROGMEM code
41-
// http://forum.arduino.cc/index.php?topic=129407.0
42-
#ifdef __AVR__
43-
#include <avr/pgmspace.h>
44-
#elif defined(ESP32)
45-
#include <pgmspace.h>
46-
#else
47-
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
48-
#ifndef __PGMSPACE_H_
49-
#define __PGMSPACE_H_ 1
50-
#include <inttypes.h>
51-
52-
#define PROGMEM
53-
#define PGM_P const char *
54-
#define PSTR(str) (str)
55-
#define F(x) x
56-
57-
typedef void prog_void;
58-
typedef char prog_char;
59-
typedef unsigned char prog_uchar;
60-
typedef int8_t prog_int8_t;
61-
typedef uint8_t prog_uint8_t;
62-
typedef int16_t prog_int16_t;
63-
typedef uint16_t prog_uint16_t;
64-
typedef int32_t prog_int32_t;
65-
typedef uint32_t prog_uint32_t;
66-
67-
#define strcpy_P(dest, src) strcpy((dest), (src))
68-
#define strcat_P(dest, src) strcat((dest), (src))
69-
#define strcmp_P(a, b) strcmp((a), (b))
70-
71-
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
72-
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
73-
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
74-
#define pgm_read_float(addr) (*(const float *)(addr))
75-
76-
#define pgm_read_byte_near(addr) pgm_read_byte(addr)
77-
#define pgm_read_word_near(addr) pgm_read_word(addr)
78-
#define pgm_read_dword_near(addr) pgm_read_dword(addr)
79-
#define pgm_read_float_near(addr) pgm_read_float(addr)
80-
#define pgm_read_byte_far(addr) pgm_read_byte(addr)
81-
#define pgm_read_word_far(addr) pgm_read_word(addr)
82-
#define pgm_read_dword_far(addr) pgm_read_dword(addr)
83-
#define pgm_read_float_far(addr) pgm_read_float(addr)
84-
#endif
85-
#endif
39+
#include <stdio.h>
40+
#include <unistd.h>
41+
#include <string.h>
42+
#include <math.h>
8643

8744
/* Source is from the InvenSense MotionApps v2 demo code. Original source is
8845
* unavailable, unless you happen to be amazing as decompiling binary by
@@ -100,15 +57,9 @@ THE SOFTWARE.
10057

10158
//#define DEBUG
10259
#ifdef DEBUG
103-
#define DEBUG_PRINT(x) Serial.print(x)
104-
#define DEBUG_PRINTF(x, y) Serial.print(x, y)
105-
#define DEBUG_PRINTLN(x) Serial.println(x)
106-
#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
60+
#define DEBUG_PRINTF(...) fprintf(stderr, __VA_ARGS__)
10761
#else
108-
#define DEBUG_PRINT(x)
109-
#define DEBUG_PRINTF(x, y)
110-
#define DEBUG_PRINTLN(x)
111-
#define DEBUG_PRINTLNF(x, y)
62+
#define DEBUG_PRINTF(...)
11263
#endif
11364

11465
#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[]
@@ -131,7 +82,7 @@ THE SOFTWARE.
13182

13283
// I Only Changed this by applying all the configuration data and capturing it before startup:
13384
// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it
134-
static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
85+
static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] = {
13586
/* bank # 0 */
13687
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
13788
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
@@ -271,14 +222,14 @@ static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
271222
// I Simplified this:
272223
uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() {
273224
// reset device
274-
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
225+
DEBUG_PRINTF("\nResetting MPU6050...\n");
275226
reset();
276-
delay(30); // wait after reset
227+
usleep(30000); // wait after reset
277228

278229
// enable sleep mode and wake cycle
279-
/*Serial.println(F("Enabling sleep mode..."));
230+
/*Serial.println("Enabling sleep mode...");
280231
setSleepEnabled(true);
281-
Serial.println(F("Enabling wake cycle..."));
232+
Serial.println("Enabling wake cycle...");
282233
setWakeCycleEnabled(true);*/
283234

284235
// disable sleep mode
@@ -287,51 +238,47 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() {
287238
// get MPU hardware revision
288239
setMemoryBank(0x10, true, true);
289240
setMemoryStartAddress(0x06);
290-
DEBUG_PRINTLN(F("Checking hardware revision..."));
291-
DEBUG_PRINT(F("Revision @ user[16][6] = "));
292-
DEBUG_PRINTLN(readMemoryByte());
293-
DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
241+
DEBUG_PRINTF("Checking hardware revision...\n");
242+
DEBUG_PRINTF("Revision @ user[16][6] = %hhx\n", readMemoryByte());
243+
DEBUG_PRINTF("Resetting memory bank selection to 0...\n");
294244
setMemoryBank(0, false, false);
295245

296246
// check OTP bank valid
297-
DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
298-
DEBUG_PRINT(F("OTP bank is "));
299-
DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!"));
247+
DEBUG_PRINTF("Reading OTP bank valid flag...\n");
248+
DEBUG_PRINTF("OTP bank is %s\n", getOTPBankValid() ? "valid!" : "invalid!");
300249

301250
// setup weird slave stuff (?)
302-
DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
251+
DEBUG_PRINTF("Setting slave 0 address to 0x7F...\n");
303252
setSlaveAddress(0, 0x7F);
304-
DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
253+
DEBUG_PRINTF("Disabling I2C Master mode...\n");
305254
setI2CMasterModeEnabled(false);
306-
DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
255+
DEBUG_PRINTF("Setting slave 0 address to 0x68 (self)...\n");
307256
setSlaveAddress(0, 0x68);
308-
DEBUG_PRINTLN(F("Resetting I2C Master control..."));
257+
DEBUG_PRINTF("Resetting I2C Master control...\n");
309258
resetI2CMaster();
310-
delay(20);
311-
DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
259+
usleep(20000);
260+
DEBUG_PRINTF("Setting clock source to Z Gyro...\n");
312261
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
313262

314-
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
263+
DEBUG_PRINTF("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
315264
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
316265

317-
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
266+
DEBUG_PRINTF("Setting sample rate to 200Hz...\n");
318267
setRate(4); // 1khz / (1 + 4) = 200 Hz
319268

320-
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
269+
DEBUG_PRINTF("Setting external frame sync to TEMP_OUT_L[0]...\n");
321270
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
322271

323-
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
272+
DEBUG_PRINTF("Setting DLPF bandwidth to 42Hz...\n");
324273
setDLPFMode(MPU6050_DLPF_BW_42);
325274

326-
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
275+
DEBUG_PRINTF("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
327276
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
328277

329278
// load DMP code into memory banks
330-
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
331-
DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
332-
DEBUG_PRINTLN(F(" bytes)"));
279+
DEBUG_PRINTF("Writing DMP code to MPU memory banks (%u bytes)\n", MPU6050_DMP_CODE_SIZE);
333280
if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Failed
334-
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
281+
DEBUG_PRINTF("Success! DMP code written and verified.\n");
335282

336283
// Set the FIFO Rate Divisor int the DMP Firmware Memory
337284
unsigned char dmpUpdate[] = {0x00, MPU6050_DMP_FIFO_RATE_DIVISOR};
@@ -342,35 +289,35 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() {
342289
//write start address LSB into register
343290
setDMPConfig2(0x00);
344291

345-
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
292+
DEBUG_PRINTF("Clearing OTP Bank flag...\n");
346293
setOTPBankValid(false);
347294

348-
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
295+
DEBUG_PRINTF("Setting motion detection threshold to 2...\n");
349296
setMotionDetectionThreshold(2);
350297

351-
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
298+
DEBUG_PRINTF("Setting zero-motion detection threshold to 156...\n");
352299
setZeroMotionDetectionThreshold(156);
353300

354-
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
301+
DEBUG_PRINTF("Setting motion detection duration to 80...\n");
355302
setMotionDetectionDuration(80);
356303

357-
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
304+
DEBUG_PRINTF("Setting zero-motion detection duration to 0...\n");
358305
setZeroMotionDetectionDuration(0);
359-
DEBUG_PRINTLN(F("Enabling FIFO..."));
306+
DEBUG_PRINTF("Enabling FIFO...\n");
360307
setFIFOEnabled(true);
361308

362-
DEBUG_PRINTLN(F("Resetting DMP..."));
309+
DEBUG_PRINTF("Resetting DMP...\n");
363310
resetDMP();
364311

365-
DEBUG_PRINTLN(F("DMP is good to go! Finally."));
312+
DEBUG_PRINTF("DMP is good to go! Finally.\n");
366313

367-
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
314+
DEBUG_PRINTF("Disabling DMP (you turn it on later)...\n");
368315
setDMPEnabled(false);
369316

370-
DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
317+
DEBUG_PRINTF("Setting up internal 42-byte (default) DMP packet buffer...\n");
371318
dmpPacketSize = 42;
372319

373-
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
320+
DEBUG_PRINTF("Resetting FIFO and clearing INT status one last time...\n");
374321
resetFIFO();
375322
getIntStatus();
376323

@@ -557,9 +504,9 @@ uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *
557504
data[2] = atan2(gravity -> y , gravity -> z);
558505
if (gravity -> z < 0) {
559506
if(data[1] > 0) {
560-
data[1] = PI - data[1];
507+
data[1] = M_PI - data[1];
561508
} else {
562-
data[1] = -PI - data[1];
509+
data[1] = -M_PI - data[1];
563510
}
564511
}
565512
return 0;

‎LinuxI2CDev/MPU6050/helper_3dmath.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ THE SOFTWARE.
3232
#ifndef _HELPER_3DMATH_H_
3333
#define _HELPER_3DMATH_H_
3434

35+
#include <math.h>
36+
3537
class Quaternion {
3638
public:
3739
float w;
@@ -213,4 +215,4 @@ class VectorFloat {
213215
}
214216
};
215217

216-
#endif /* _HELPER_3DMATH_H_ */
218+
#endif /* _HELPER_3DMATH_H_ */

0 commit comments

Comments
 (0)