Estimates vehicle speed from throttle percentage using calibration data. More...

#include <SpeedFromThrottle.h>

Inheritance diagram for SpeedFromThrottle:

[legend]

Collaboration diagram for SpeedFromThrottle:

[legend]

Public Member Functions
	SpeedFromThrottle (float maxSpeedMps, uint8_t numMotors=1)

	SpeedFromThrottle (Speed maxSpeed, uint8_t numMotors=1)

void	setMaxSpeed (Speed speed)

bool	onMessage (const Message< float > &msg) override
	Handle Throttle messages to update speed.

void	addSpeedCalibration (float throttlePercent, float speedMps)
	Add or update a calibration point (throttlePercent, speedMps)

void	clearCalibration ()
	Clear all calibration data.

void	setThrottlePercent (float throttlePercent, uint8_t motor=0) override
	Define the actual throttle value.

Speed	getSpeed (uint8_t motor=0) const override
	Get the actual speed based on the last throttle value.

Speed	updateSpeed (uint32_t deltaTimeMs, uint8_t motor=0) override
	For sources with inertia, call this in your main loop with the elapsed time (in milliseconds) to update the speed estimate for a specific motor.

size_t	getMotorCount () const override

Public Member Functions inherited from MessageSource
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.

Public Member Functions inherited from MessageHandler
virtual bool	onMessage (const Message< float > &msg)=0
	Handle an incoming message (pure virtual).

virtual bool	onMessage (const Message< Coordinate< float > > &msg)

virtual bool	onMessage (const Message< GPSCoordinate > &msg)

virtual bool	onMessage (const Message< MotionState3D > &msg)

virtual Speed	getSpeed (uint8_t motor=0) const =0
	Get the current speed.

virtual void	setThrottlePercent (float value, uint8_t motor=0)=0
	Publish actual speed for a specific motor.

virtual Speed	updateSpeed (uint32_t deltaTimeMs, uint8_t motor=0)=0
	For sources with inertia, call this in your main loop with the elapsed time (in milliseconds) to update the speed estimate for a specific motor.

virtual size_t	getMotorCount () const =0

Protected Member Functions
bool	init ()

void	sendSpeedMessage (uint8_t motor=0)

float	getSpeedMPS (float throttlePercent) const
	Get speed in meters per second by interpolating calibration data.

Protected Attributes
std::vector< std::pair< float, float > >	calibrationData

std::vector< float >	speedMps

uint8_t	numMotors = 1

float	maxSpeedMps = 0.0f

Protected Attributes inherited from MessageSource
std::vector< MessageHandlerEntry >	messageHandlers_

Detailed Description

Estimates vehicle speed from throttle percentage using calibration data.

This class provides a flexible, piecewise linear mapping from throttle percentage (e.g., -100% to 100%) to speed (in meters per second), based on user-provided calibration points. It is useful for systems where direct speed measurement is unavailable or unreliable, and a throttle-to-speed relationship can be established empirically. The mapping can be linear or non-linear, depending on the calibration data.

Features:

By default, 0% throttle maps to 0 m/s, 100% throttle maps to maxSpeedMps, and -100% throttle maps to -maxSpeedMps.
Additional calibration points can be added with addSpeedCalibration(throttle, speed), allowing for non-linear mappings.
If a calibration point for a throttle value already exists, it is replaced; otherwise, it is inserted in order.
Supports both direct speed queries (getSpeed, getSpeedMPS) and integration with the TinyRobotics message system.
When used as a MessageHandler, it listens for Throttle messages and emits Speed messages via MessageSource.
Calibration data can be cleared and redefined at runtime.

Typical usage:

  SpeedFromThrottle speedMap(2.0f); // 2 m/s at 100% throttle
  speedMap.addSpeedCalibration(50.0f, 1.0f); // 1 m/s at 50%
  float speed = speedMap.getSpeedMPS(75.0f); // Interpolated speed
  // As a message handler/source:
  bus.subscribe(speedMap); // Receives Throttle messages, emits Speed
messages

Integration:

Use as a standalone utility for throttle-to-speed conversion.
Or, connect to a MessageBus to automatically convert Throttle messages to Speed messages.