servo_sync.c File Reference

Search that synchronises two servos on one surface by total current. More...

#include "servo_sync.h"
#include <string.h>

Functions
static bool	elapsed (uint32_t now, uint32_t then, uint32_t ms)
	Tell whether a millisecond span has elapsed.
void	servo_sync_defaults (servo_sync_cfg_t *cfg, uint16_t centre_us)
	Defaults for a sync search.
static uint16_t	pulse_us (int32_t us)
	A command within the pulse widths a servo takes.
static int32_t	point_us (const servo_sync_t *sy, uint8_t i)
	Where in its window the scan's i-th point sits.
static void	aim (servo_sync_t *sy)
	Where the servos should be for the stage being measured.
void	servo_sync_init (servo_sync_t *sy, const servo_sync_cfg_t *cfg)
	Start a sync search at the centre stage.
static void	finish (servo_sync_t *sy, servo_sync_state_t state, servo_sync_fault_t fault)
	End the search with the servos back at centre.
static void	next_stage (servo_sync_t *sy)
	Start the next stage, or finish if that was the last.
static void	close_round (servo_sync_t *sy)
	A whole round of points has been measured; narrow, or take the answer.
void	servo_sync_step (servo_sync_t *sy, float total_a, uint32_t now_ms, uint16_t *cmd_a, uint16_t *cmd_b)
	Advance the search with one reading of the pair's total current.
float	servo_sync_progress (const servo_sync_t *sy)
	0..1 across all three stages.

Detailed Description

Search that synchronises two servos on one surface by total current.

SPDX-License-Identifier: MIT

Function Documentation

aim()

void aim ( servo_sync_t * sy )

static

Where the servos should be for the stage being measured.

Parameters

sy	Search state. Sets cmd_a_us, cmd_b_us and travel_ms.

close_round()

void close_round ( servo_sync_t * sy )

static

A whole round of points has been measured; narrow, or take the answer.

Parameters

sy	Search state holding scan_a for the round.

elapsed()

bool elapsed ( uint32_t now, uint32_t then, uint32_t ms )

inlinestatic

Tell whether a millisecond span has elapsed.

Parameters

now	Current time, ms.
then	Start of the span, ms.
ms	Span length, ms.

Returns

true when now - then is at least ms, across a counter wrap.

finish()

void finish ( servo_sync_t * sy, servo_sync_state_t state, servo_sync_fault_t fault )

static

End the search with the servos back at centre.

Parameters

sy	Search state. Sets state, fault and the two commands.
state	Final state.
fault	Fault code.

next_stage()

void next_stage ( servo_sync_t * sy )

static

Start the next stage, or finish if that was the last.

Parameters

sy	Search state.

point_us()

int32_t point_us ( const servo_sync_t * sy, uint8_t i )

static

Where in its window the scan's i-th point sits.

Parameters

sy	Search state holding the window.
i	Point index, 0 to points - 1.

Returns

offset of the point within the window, us.

pulse_us()

uint16_t pulse_us ( int32_t us )

static

A command within the pulse widths a servo takes.

Parameters

us	Pulse width, us.

Returns

the width, clamped to SERVO_SYNC_MIN_US to SERVO_SYNC_MAX_US.

servo_sync_defaults()

void servo_sync_defaults	(	servo_sync_cfg_t *	cfg,
		uint16_t	centre_us )

Defaults for a sync search.

Parameters

cfg	Configuration to fill. Does nothing when NULL.
centre_us	Centre pulse width, us.

servo_sync_init()

void servo_sync_init	(	servo_sync_t *	sy,
		const servo_sync_cfg_t *	cfg )

Start a sync search at the centre stage.

Parameters

sy	Search state to initialise. Does nothing when NULL.
cfg	Configuration to copy. Does nothing when NULL.

servo_sync_progress()

float servo_sync_progress

(

const servo_sync_t *

sy

)

0..1 across all three stages.

Parameters

sy	Search state. NULL gives 0.0.

Returns

0.0 to 1.0; 1.0 once the search is not RUNNING.

servo_sync_step()

void servo_sync_step	(	servo_sync_t *	sy,
		float	total_a,
		uint32_t	now_ms,
		uint16_t *	cmd_a,
		uint16_t *	cmd_b )

Advance the search with one reading of the pair's total current.

Total, not per servo: the quantity minimised is what the two draw together, so one sensor across both outputs is enough.

Writes the two pulse widths to command through cmd_a and cmd_b.

Parameters

sy	Search state.
total_a	Total current of the pair, A.
now_ms	Time of the reading, ms.
cmd_a	Out: pulse width for servo A, us. NULL to skip.
cmd_b	Out: pulse width for servo B, us. NULL to skip.