sts_protocol.c File Reference

: Feetech STS Servo Protocol Implementation More...

#include <stdint.h>
#include <stddef.h>
#include <string.h>
#include "sts_protocol.h"

Functions
sts_result_t	sts_calculate_checksum (const uint8_t *pkt_buf, uint16_t pkt_len, uint8_t *checksum_out)
	Calculates the Feetech STS Checksum. Sums ID, Length, Instruction, and Parameters (skipping the initial 0xFF 0xFF headers), then returns the bitwise NOT of the 8-bit truncation.
sts_result_t	sts_create_packet (uint8_t id, uint8_t instruction, const uint8_t *param_buf, uint16_t param_len, uint8_t *pkt_buf, uint16_t pkt_buf_size)
	Constructs an STS packet in the provided buffer.
sts_result_t	sts_parse_response (uint8_t expected_id, const uint8_t *rx_buf, uint16_t rx_len, uint8_t *param_buf, uint16_t param_buf_size, uint16_t *param_len)
	Parses a response packet from a servo, searching for valid headers in the buffer.

Detailed Description

: Feetech STS Servo Protocol Implementation

Author

: Grisham Balloo

Date

: 2026-03-06

This module implements the core logic for encoding and decoding Feetech STS series servo packets.

Key Features:

1. Checksum Calculation: Implements the 8-bit NOT-sum logic required by the Feetech hardware specification.
2. Packet Creation: Encodes instructions and parameters into valid, ready-to-transmit binary packets.
3. Response Parsing: Synchronizes and extracts valid servo data from a raw byte stream, filtered for errors and noise.

Attention

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

Function Documentation

sts_calculate_checksum()

sts_result_t sts_calculate_checksum	(	const uint8_t *	pkt_buf,
		uint16_t	pkt_len,
		uint8_t *	checksum_out
	)

Calculates the Feetech STS Checksum. Sums ID, Length, Instruction, and Parameters (skipping the initial 0xFF 0xFF headers), then returns the bitwise NOT of the 8-bit truncation.

Parameters

[in]	pkt_buf	Pointer to the start of the packet buffer.
[in]	pkt_len	Total packet length to process.
[out]	checksum_out	Pointer to store the 8-bit NOT-sum result.

Returns

sts_result_t STS_OK on success, error code otherwise

sts_create_packet()

sts_result_t sts_create_packet	(	uint8_t	id,
		uint8_t	instruction,
		const uint8_t *	param_buf,
		uint16_t	param_len,
		uint8_t *	pkt_buf,
		uint16_t	pkt_buf_size
	)

Constructs an STS packet in the provided buffer.

Parameters

id	Servo ID (0-253, 254 for broadcast).
instruction	STS Instruction (e.g., 0x01 for Ping) or Status code.
param_buf	Pointer to parameter bytes (can be NULL if param_len is 0).
param_len	Number of parameters.
pkt_buf	The output buffer to store the packet.
pkt_buf_size	The capacity of the output buffer.

Returns

sts_result_t STS_OK or error code.

sts_parse_response()

sts_result_t sts_parse_response	(	uint8_t	expected_id,
		const uint8_t *	rx_buf,
		uint16_t	rx_len,
		uint8_t *	param_buf,
		uint16_t	param_buf_size,
		uint16_t *	param_len
	)

Parses a response packet from a servo, searching for valid headers in the buffer.

This function will skip invalid data until a valid packet for the expected ID is found or the buffer is exhausted.

Parameters

expected_id	The ID we are expecting a response from.
rx_buf	The raw data received from the UART.
rx_len	The length of data in rx_buf.
param_buf	[Out] Buffer to store extracted parameters.
param_buf_size	The capacity of the param_buf buffer.
param_len	[Out] Number of bytes actually extracted.

Returns

sts_result_t STS_OK on success, or relevant error code.