|
|
bool | begin (T initialAngleDeg, Coordinate< DistanceM > initialPosition) |
| |
| bool | begin (Transform2D transform) override |
| |
| void | end () |
| |
| void | update (T accelX, T accelY, T gyroZ_in, unsigned long nowMillis) |
| |
|
Delta2D | getLastDelta () const |
| | Get the last delta update (dx, dy, dtheta)
|
| |
|
float | getAngularVelocity () const |
| | Get the current angular velocity (radians/second)
|
| |
|
float | getLinearVelocity () const |
| | Get the current linear velocity (meters/second)
|
| |
|
float | getTheta () const |
| | Get the current orientation (radians)
|
| |
| Coordinate< DistanceM > | getPosition () const |
| | get position
|
| |
| Angle | getHeading () const |
| | get heading as Angle
|
| |
| Speed | getSpeed () const |
| | get speed as Speed
|
| |
|
Distance | getTotalDistance () const |
| | Get the total distance traveled.
|
| |
|
uint32_t | getTimestamp () const |
| | Get time of last update.
|
| |
| Transform2D | getTransform () const |
| |
|
void | publish () |
| |
| virtual Coordinate< DistanceM > | getPosition () const =0 |
| |
| virtual Angle | getHeading () const =0 |
| |
| virtual Speed | getSpeed () const =0 |
| |
|
virtual Transform2D | getTransform () const =0 |
| |
|
virtual bool | begin (Transform2D transform)=0 |
| |
|
virtual void | end ()=0 |
| |
| void | subscribe (MessageHandler &handler, MessageOrigin origin=MessageOrigin::Undefined, MessageContent content=MessageContent::Undefined) |
| | Subscribe a message handler to this source, with optional filtering.
|
| |
|
void | unsubscribeAll () |
| | Remove all registered message handlers.
|
| |
| void | sendMessage (Message< float > &msg) |
| | Publish a message to all registered handlers.
|
| |
| void | sendMessage (const Message< Coordinate< float > > &msg) |
| | Publish a message to all registered handlers.
|
| |
| void | sendMessage (const Message< GPSCoordinate > &msg) |
| | Publish a message to all registered handlers.
|
| |
|
void | sendMessage (const Message< MotionState3D > &msg) |
| | Overload for MotionState3D messages.
|
| |
template<typename T = float>
class tinyrobotics::IMU2D< T >
Basic 2D IMU dead-reckoning class.
This class estimates 2D position, velocity, and heading (yaw angle) for robotics applications by integrating gyroscope and accelerometer data over time.
- Integrates gyro Z (yaw rate) to estimate heading (radians)
- Integrates accelerometer (X, Y) in the robot frame, rotated to world frame, to estimate velocity and position
- All units SI (meters, seconds, radians)
Limitations:
- No sensor fusion with magnetometer or GPS
- No Kalman filter or bias estimation
- Subject to drift over time (no correction)
Example usage:
imu.begin(initialAngle, initialPosition);
imu.
update(accelX, accelY, gyroZ, millis());
Basic 2D IMU dead-reckoning class.
Definition: IMU2D.h:47
void update(T accelX, T accelY, T gyroZ_in, unsigned long nowMillis)
Definition: IMU2D.h:68
Angle getHeading() const
get heading as Angle
Definition: IMU2D.h:121
Speed getSpeed() const
get speed as Speed
Definition: IMU2D.h:123
Coordinate< DistanceM > getPosition() const
get position
Definition: IMU2D.h:119
- Template Parameters
-
| T | Numeric type (float or double recommended) |