Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add MPU6050 library and sample code #2655

Merged
merged 21 commits into from
Nov 9, 2023

Conversation

xeonqq
Copy link
Contributor

@xeonqq xeonqq commented Jul 28, 2023

No description provided.

@what-the-diff
Copy link

what-the-diff bot commented Jul 28, 2023

PR Summary

  • Introduction of MPU6050 library
    This version adds a new directory for the MPU6050 library. It removes redundant functionalities and includes process simplifications in the form of using built-in functions.
  • Dependency on I2Cdev component
    The MPU6050 library now relies on the I2Cdev component, which means they work together and changes in one could impact the other.
  • 3D Math Calculations Enhancement
    Additionally, we have implemented helper math functions and classes to boost our 3D math calculations.
  • Accelerometer and Gyroscope Reader Sample
    A new sample module for reading and printing the accelerometer and gyroscope values from MPU6050 has been added which should serve as a practical reference for usage.
  • Sample Module Dependencies and Configuration
    The sample module is dependent on our new MPU6050 library and specifically excludes network functionalities.
  • Visual Context
    For a better understanding, an image file of the MPU6050 sensor has been included.

Copy link
Contributor

@mikee47 mikee47 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A few thoughts

Comment on lines 10 to 21
Serial.print("a/g:\t");
Serial.print(ax);
Serial.print("\t");
Serial.print(ay);
Serial.print("\t");
Serial.print(az);
Serial.print("\t");
Serial.print(gx);
Serial.print("\t");
Serial.print(gy);
Serial.print("\t");
Serial.println(gz);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Or Serial << "a/g:\t" << ax << "\t" << ay << "\t" << az << "\t" << gx << "\t" << gy << "\t" << gz << endl;

z /= m;
}

VectorFloat getNormalized()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

const

}
};

class VectorInt16
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

VectorInt16 and VectorFloat are essentially duplicates; use class template.

template <typename T> class Vector
{
public:
	T x;
	T y;
	T z;

	Vector(): x{}, y{}, z{}
	{
	}

	Vector(T nx, T ny, T nz): x(nx), y(ny), z(nz)
	{
	}

	// Explicit copy constructor
	Vector(const Vector& other): x(other.x), y(other.y), z(other.z)
	{
	}

	float getMagnitude()
	{
		return sqrt(x * x + y * y + z * z);
	}

	void normalize()
	{
		auto m = getMagnitude();
		// MUST check for divide-by-zero
		if(m) {
	            x /= m;
	            y /= m;
	            z /= m;
	        }
	}

	Vector getNormalized()
	{
		Vector r(*this);
		r.normalize();
		return r;
	}

	// NOTE: Use reference parameter, not pointer
	void rotate(const Quaternion& q)
	{
		// http://www.cprogramming.com/tutorial/3d/quaternions.html
		// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
		// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
		// ^ or:
		// http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1

		// P_out = q * P_in * conj(q)
		// - P_out is the output vector
		// - q is the orientation quaternion
		// - P_in is the input vector (a*aReal)
		// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z],
		// q*=[w,-x,-y,-z])
		Quaternion p(0, x, y, z);

		// quaternion multiplication: q * p, stored back in p
		p = q.getProduct(p);

		// quaternion multiplication: p * conj(q), stored back in p
		p = p.getProduct(q.getConjugate());

		// p quaternion is now [0, x', y', z']
		x = p.x;
		y = p.y;
		z = p.z;
	}

	Vector getRotated(const Quaternion& q) const
	{
		Vector r(*this);
		r.rotate(q);
		return r;
	}
};

using VectorInt16 = Vector<int16_t>;
using VectorFloat = Vector<float>;

w * q.z + x * q.y - y * q.x + z * q.w); // new z
}

Quaternion getConjugate()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Quaternion getConjugate() const

z = nz;
}

Quaternion getProduct(Quaternion q)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Quaternion getProduct(const Quaternion& q) Use pass-by-reference instead of copy

return Quaternion(w, -x, -y, -z);
}

float getMagnitude()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

float getMagnitude() const

Comment on lines 45 to 59
Quaternion()
{
w = 1.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
}

Quaternion(float nw, float nx, float ny, float nz)
{
w = nw;
x = nx;
y = ny;
z = nz;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use static initialization in constructors

Quaternion(): w(1), x(0), y(0), z(0)
{
}

Quaternion(float nw, float nx, float ny, float nz): w(nw), x(nx), y(ny), z(nz)
{
}

Also add copy constructor:

Quaternion(const Quaternion& other): w(other.w), x(other.x), y(other.y), z(other.z)
{
}

z /= m;
}

Quaternion getNormalized()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Quaternion getNormalized() const


Quaternion getNormalized()
{
Quaternion r(w, x, y, z);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Quaternion r(*this);

mpu.initialize();
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

mainLoopTimer.initializeMs(static_cast<int>(mainLoopInterval * 1000), mainLoop).start();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

or mainLoopTimer.initializeMs<mainLoopInterval * 1000>(mainLoop).start(); as mainLoopInterval is declared constexpr

@xeonqq
Copy link
Contributor Author

xeonqq commented Jul 28, 2023

After a closer examination, I realized the quaternion related code is not used in this part of the library (see here). So I simply deleted it to keep the library lean.

The other comments of yours I have addressed. Thank you for the feedback. Please have a look again~

@xeonqq xeonqq requested a review from mikee47 July 28, 2023 16:21
@xeonqq xeonqq changed the title Add MPU6050 library and sample code {WIP} Add MPU6050 library and sample code Jul 29, 2023
@xeonqq xeonqq changed the title {WIP} Add MPU6050 library and sample code Add MPU6050 library and sample code Jul 29, 2023
@slaff
Copy link
Contributor

slaff commented Jul 29, 2023

@mikee47 The build with IDF v5.0 is failing. I was able to reproduce the issue on my machine and it seems to not be related to this PR. Can you check if we have to downgrade/upgrade the pyparse version?

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen.py", line 183, in <module>
    main()
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen.py", line 159, in main
    mapping_rules = generation_model.generate(sections_infos)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 514, in generate
    root_node.insert(entity, sections, target, flags, entities)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 241, in insert
    self.child_placement(entity, sections, target, flags, sections_db)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 233, in child_placement
    child.insert(entity, sections, target, flags, sections_db)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 241, in insert
    self.child_placement(entity, sections, target, flags, sections_db)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 233, in child_placement
    child.insert(entity, sections, target, flags, sections_db)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 241, in insert
    self.child_placement(entity, sections, target, flags, sections_db)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/generation.py", line 296, in child_placement
    found_sections = sections_db.get_sections(self.parent.name, self.name)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/entity.py", line 197, in get_sections
    obj = self._match_obj(archive, obj)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/entity.py", line 183, in _match_obj
    objs = self.get_objects(archive)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/entity.py", line 176, in get_objects
    self._process_archive(archive)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/entity.py", line 164, in _process_archive
    parsed = self._get_infos_from_file(stored)
  File "/opt/esp-idf-5.0/tools/ldgen/ldgen/entity.py", line 154, in _get_infos_from_file
    raise ParseException('Unable to parse section info file ' + info.filename + '. ' + p.msg)
pyparsing.exceptions.ParseException: Unable to parse section info file /home/runner/work/Sming/Sming/Sming/out/Esp32/esp32/debug/build/esp32/sdk/esp-idf/esp_event/libesp_event.a. Expected end of text  (at char 0), (line:1, col:1)
ninja: build stopped: subcommand failed.
Error: Process completed with exit code 2.

Copy link
Contributor

@mikee47 mikee47 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is an example of code which got written with a 'C' viewpoint rather than C++. More robust, easier to manage and safer to use references instead of pointers, structures instead of long parameter lists, etc. Hopefully this all makes sense?

/** Default constructor, uses default I2C address.
* @see MPU6050_DEFAULT_ADDRESS
*/
MPU6050::MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Trivial methods should be moved to header (compiler cannot optimise what it cannot see)

* @see MPU6050_ADDRESS_AD0_LOW
* @see MPU6050_ADDRESS_AD0_HIGH
*/
MPU6050::MPU6050(uint8_t address) : devAddr{address}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Move to header

Comment on lines 79 to 83
bool MPU6050::testConnection()
{
return getDeviceID() == 0x34;
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Move to header ?

Comment on lines 1041 to 115
if(num > 3)
return 0;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use braces for conditionals:

if(num > 3) {
  return 0;
}

Comment on lines 1054 to 123
if(num > 3)
return;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Braces

Comment on lines 177 to 178
I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
return buffer[0];
Copy link
Contributor

@mikee47 mikee47 Jul 29, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using helper

return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH);

Comment on lines 293 to 295
I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]);
I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);
return (buffer[0] >> 3) | ((buffer[1] >> 2) & 0x03);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using helpers:

uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
uint8_t a = readByte(MPU6050_RA_SELF_TEST_A);
return (y >> 3) | ((a >> 2) & 0x03);

@@ -0,0 +1 @@
COMPONENT_DEPENDS := I2Cdev

This comment was marked as outdated.

#define MPU6050_DMP_MEMORY_BANK_SIZE 256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16

// note: DMP code memory blocks defined at end of header file
Copy link
Contributor

@mikee47 mikee47 Jul 29, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove comment (or edit to make sense)

uint8_t buffer[14] = {0};
};

#endif /* _MPU6050_H_ */
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove (#pragma once)

@slaff slaff added this to the 5.1.0 milestone Aug 1, 2023
@xeonqq
Copy link
Contributor Author

xeonqq commented Aug 1, 2023

Thanks for the review, I agree the code which I copy/paste is not to the standard of Sming. I will do the refactoring when I find time. Currently I want to first prototype the balance robot I am working on:)

@xeonqq xeonqq changed the title Add MPU6050 library and sample code [WIP] Add MPU6050 library and sample code Sep 10, 2023
@slaff
Copy link
Contributor

slaff commented Sep 12, 2023

@xeonqq are you ready with all changes to this PR?

@xeonqq
Copy link
Contributor Author

xeonqq commented Sep 13, 2023

@xeonqq are you ready with all changes to this PR?

No, not yet, there are still many comments I didn't address yet. Quite some workload. I will remove the wip tag in the PR title once I am ready for another review.

For simplicity and code quality, they are deleted at the moment
@xeonqq xeonqq changed the title [WIP] Add MPU6050 library and sample code Add MPU6050 library and sample code Nov 4, 2023
@xeonqq
Copy link
Contributor Author

xeonqq commented Nov 4, 2023

@mikee47 ready for review again. Sorry for the delayed PR.
I in the end deleted Calibration and Memory Block related code for simplicity and code quality reason, anyway that is not frequently used functionality.
I fail one of the CI checks, it's about some clock cycles I am not sure if it's related to my change.

@mikee47
Copy link
Contributor

mikee47 commented Nov 7, 2023

I fail one of the CI checks, it's about some clock cycles I am not sure if it's related to my change.

Does that sometimes, host emulator isn't perfect :-( Re-running the test

Comment on lines 1 to 14
API Documentation
-----------------

.. doxygenclass:: MPU6050
:members:

# MPU6050 Six-Axis (Gyro + Accelerometer)
Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050) @ 605a740. Most of the code is the same, except:

- Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
- MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.
- Removed map function in favor of the Sming built-in one.
- Adapted include path, coding style and applied clangformat
- Deleted Calibration and Memory Block related code for code quality reason
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Have a look at other libraries/Components for general layout. Heading must be double-underlined (level 1) and give a good name for the library. For example:

MPU6050 Gyro / Accelerometer
============================

Follow this with the description. Note that .rst files (restructured text) syntax differs from markdown so remember to fix links, e.g.

`jrowberg/i2cdevlib<https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>`

And put the API section last.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I took look of some, but they are not very consistent. I do get confused on how to write it properly

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, takes a bit of getting used to but has advantages to markdown! See https://sming.readthedocs.io/en/latest/information/develop/documentation.html

Copy link
Contributor

@mikee47 mikee47 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You've inadvertently reverted the FlashString and IFS libraries! Other than that, and the documentation change, looks great! I assume it's all working and tested on actual hardware?

@xeonqq
Copy link
Contributor Author

xeonqq commented Nov 7, 2023

You've inadvertently reverted the FlashString and IFS libraries! Other than that, and the documentation change, looks great! I assume it's all working and tested on actual hardware?

I just realized that too, and reverted the unintended change of the submodules.

I tested on esp8266 and mpu6050, but only the functions used in the sample program. So I can't say for sure it is bullet proof.
And tested assertion is working as expected.

Comment on lines 4 to 8
:members:

# MPU6050 Six-Axis (Gyro + Accelerometer)
Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>` @ 605a740. Most of the code is the same, except:

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Almost... remove :members: and line starting '#'. Also the URL requires __ at the end!

Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>`__ @ 605a740. Most of the code is the same, except:

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

Copy link
Contributor

@slaff slaff left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fix small typos.

// Changelog:
// ... - ongoing debug release

// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fix typo, replace PARIAL with PARTIAL.

// (RM-MPU-6000A-00)
// Based On https://github.com/jrowberg/i2cdevlib

// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fix typo, replace PARIAL with PARTIAL.


const auto sz = sizeof(T);
uint8_t buffer[sz] = {0};
//data follow big endien convention
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fix typo, replace endien with endian.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

@slaff slaff merged commit 9f55c7a into SmingHub:develop Nov 9, 2023
36 checks passed
@sreggy sreggy mentioned this pull request Nov 14, 2023
@slaff slaff mentioned this pull request Dec 12, 2023
5 tasks
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants