sts_servo.c File Reference

: STS Service Layer Implementation More...

#include "sts_servo.h"
#include "sts_protocol.h"
#include "sts_registers.h"
#include <string.h>

Macros
#define	STATIC_TESTABLE   static

Functions
sts_result_t	STS_Bus_Transmit (sts_bus_t *bus, const uint8_t *data, uint16_t len)
	Safe wrapper for HAL transmission.
sts_result_t	STS_Bus_Receive (sts_bus_t *bus, uint8_t *data, uint16_t len, uint32_t timeout)
	Safe wrapper for HAL reception.
sts_result_t	STS_Bus_FlushRx (sts_bus_t *bus)
	Drains stale bytes from the RX buffer between transaction retries. No-op if the port did not register a flush_rx function.
STATIC_TESTABLE sts_result_t	sts_execute_command (sts_servo_t *servo, const sts_cmd_t *cmd)
	Single transaction path for all STS servo commands.
sts_result_t	STS_Bus_Init (sts_bus_t *bus, void *port_handle, sts_hal_transmit_t tx_func, sts_hal_receive_t rx_func)
	Initializes the hardware bus abstraction.
sts_result_t	STS_Servo_Init (sts_servo_t *servo, sts_bus_t *bus, uint8_t id)
	Initializes a servo handle.
sts_result_t	STS_servo_ping (sts_servo_t *servo)
	Pings the servo to check if it's online.
sts_result_t	STS_Write8 (sts_servo_t *servo, uint8_t reg_addr, uint8_t value)
	Writes a single byte (8-bit) to a specific servo register.
sts_result_t	STS_Write16 (sts_servo_t *servo, uint8_t reg_addr, uint16_t value)
	Writes a word (16-bit) to a specific servo register (Little-Endian).
sts_result_t	STS_Read8 (sts_servo_t *servo, uint8_t reg_addr, uint8_t *value_out)
	Reads a single byte (8-bit) from a specific servo register.
sts_result_t	STS_Read16 (sts_servo_t *servo, uint8_t reg_addr, uint16_t *value_out)
	Reads a word (16-bit) from a specific servo register (Little-Endian).

Detailed Description

: STS Service Layer Implementation

Author

: Grisham Balloo

This module implements the high-level service functions for the Feetech STS protocol. It provides a hardware-agnostic interface for motor control through the following functional areas:

• Bus HAL: STS_Bus_Init maps platform transmit/receive function pointers onto a shared bus handle.
• Servo Handle: STS_Servo_Init binds a servo ID to a bus and initialises online state to offline.
• Command Engine: sts_execute_command (STATIC_TESTABLE) is the single transaction path — handles framing, TX, and a two-stage receive. Stage 1 reads the fixed header to extract the packet length field. Stage 2 reads the remaining payload. The length field is validated against the minimum, RX buffer limit, and cmd->expected_rx_len before Stage 2 is attempted. Broadcast IDs and expected_rx_len == 0 skip the receive entirely.
• Ping: STS_servo_ping issues a PING instruction and updates is_online, distinguishing communication failures from servo hardware errors.
• Register Primitives: STS_Write8, STS_Write16, STS_Read8, STS_Read16 provide typed register access built on the command engine.

Attention

Copyright (c) 2026 Grisham Balloo. All rights reserved.

Function Documentation

STS_Bus_FlushRx()

sts_result_t STS_Bus_FlushRx

(

sts_bus_t *

bus

)

Drains stale bytes from the RX buffer between transaction retries. No-op if the port did not register a flush_rx function.

Returns

STS_OK always (flush failures are non-fatal).

STS_Bus_Receive()

sts_result_t STS_Bus_Receive	(	sts_bus_t *	bus,
		uint8_t *	data,
		uint16_t	len,
		uint32_t	timeout
	)

Safe wrapper for HAL reception.

Returns

STS_ERR_NULL_PTR if function pointer is missing, else HAL result.

STS_Bus_Transmit()

sts_result_t STS_Bus_Transmit	(	sts_bus_t *	bus,
		const uint8_t *	data,
		uint16_t	len
	)

Safe wrapper for HAL transmission.

Returns

STS_ERR_NULL_PTR if function pointer is missing, else HAL result.

sts_execute_command()

STATIC_TESTABLE sts_result_t sts_execute_command	(	sts_servo_t *	servo,
		const sts_cmd_t *	cmd
	)

Single transaction path for all STS servo commands.

Frames and transmits a command packet, then performs a two-stage receive: Stage 1 reads the fixed header to extract the packet length field. Stage 2 reads the remaining payload. The length field is validated against STS_MIN_PKT_LEN_VAL, the RX buffer limit, and cmd->expected_rx_len before Stage 2 is attempted. Skips RX entirely on broadcast IDs or expected_rx_len == 0.

Note

Not thread-safe. Add mutex protection here for RTOS support.

Parameters

servo	Pointer to an initialised servo handle.
cmd	Pointer to the command configuration struct.

Returns

STS_OK or specific error code

STS_Servo_Init()

sts_result_t STS_Servo_Init	(	sts_servo_t *	servo,
		sts_bus_t *	bus,
		uint8_t	id
	)

Initializes a servo handle.

Parameters

servo	Pointer to the servo handle to initialize.
bus	Pointer to an initialized bus handle.
id	The hardware ID of the servo (must be < 254).

Returns

STS_OK on success, STS_ERR_NULL_PTR if servo/bus are NULL, STS_ERR_INVALID_PARAM if ID is 254 or 255.

STS_servo_ping()

sts_result_t STS_servo_ping

(

sts_servo_t *

servo

)

Pings the servo to check if it's online.

Parameters

servo

Pointer to the initialized servo handle.

Returns

STS_OK if the servo responds correctly, error code otherwise. On success, the servo's is_online field is set to STS_ONLINE; on failure, it is set to STS_OFFLINE.