36#error "STS3032_IDS must be defined"
40#define STS3032_DEBUG FALSE
43#ifndef STS3032_RESPONSE_LEVEL
44#define STS3032_RESPONSE_LEVEL 0
50#ifndef STS3032_DELAY_MSG
51#define STS3032_DELAY_MSG 1000
54#ifndef STS3032_DELAY_MSG_MIN
55#define STS3032_DELAY_MSG_MIN 0
58#define BUF_MAX_LENGHT 15
90 sts.periph = &(STS3032_DEV);
92 sts.nb_bytes_expected = 1;
93 sts.nb_failed_checksum = 0;
96 static const uint8_t tmp_ids[SERVOS_STS3032_NB] = STS3032_IDS;
97 memcpy(
sts.ids, tmp_ids,
sizeof(tmp_ids));
108 for (
int i = 0; i < SERVOS_STS3032_NB; i++) {
113 float data[SERVOS_STS3032_NB + 1];
114 data[0] =
sts.nb_failed_checksum;
115 for (
int i = 0; i < SERVOS_STS3032_NB; i++) {
116 data[i + 1] =
sts.pos[i];
143 sts->wait_reply = buf[0];
144 sts->time_last_msg = now;
150 switch (
sts->rx_state) {
153 sts->buf_header[1] =
sts->buf_header[0];
155 if (
sts->buf_header[0] == 0xFF &&
sts->buf_header[1] == 0xFF) {
157 sts->nb_bytes_expected = 2;
158 sts->buf_header[0] = 0;
159 sts->buf_header[1] = 0;
167 sts->rx_checksum =
sts->rx_id +
sts->nb_bytes_expected;
175 for (
int i = 0; i <
sts->nb_bytes_expected - 2; i++) {
180 sts->rx_checksum =
~sts->rx_checksum;
186 sts->nb_failed_checksum++;
190 sts->nb_bytes_expected = 1;
211 if (
offset + 2 < length) {
228 for (
int i = 0; i < SERVOS_STS3032_NB; i++) {
229 if (
sts->ids[i] ==
id) {
268 position = -position;
269 position |= (1 << 15);
273 buf[1] = position & 0xff;
274 buf[2] = position >> 8;
295 sts->read_addr = addr;
310 memcpy(&buf[6], data, len);
311 for (
int i = 0; i < len; i++) {
void actuators_sts3032_set_id(float future_id)
void actuators_sts3032_set(uint8_t idx, int16_t value)
int sts3032_response_level
void actuators_sts3032_init(void)
void sts3032_enable_torque(struct sts3032 *sts, uint8_t id, uint8_t enable)
static void write_buf(struct sts3032 *sts, uint8_t id, uint8_t *data, uint8_t len, uint8_t fun, uint8_t reply)
uint8_t cbuf[50 *SERVOS_STS3032_NB]
void actuators_sts3032_set_response_level(float level)
void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position)
#define STS3032_RESPONSE_LEVEL
void sts3032_read_mem(struct sts3032 *sts, uint8_t id, uint8_t addr, uint8_t len)
void sts3032_lock_eeprom(struct sts3032 *sts, uint8_t id, uint8_t lock)
void sts3032_read_pos(struct sts3032 *sts, uint8_t id)
void actuators_sts3032_event(void)
void actuators_sts3032_move(float pos)
void sts3032_init(struct sts3032 *sts, struct uart_periph *periph, uint8_t *cbuf, size_t cbuf_len)
void actuators_sts3032_lock_eprom(float lock)
#define STS3032_DELAY_MSG
uint8_t id_idx(struct sts3032 *sts, uint8_t id)
#define STS3032_DELAY_MSG_MIN
void actuators_sts3032_periodic(void)
static void handle_reply(struct sts3032 *sts, uint8_t id, uint8_t state, uint8_t *buf_param, uint8_t length)
void sts3032_set_id(struct sts3032 *sts, uint8_t id, uint8_t new_id)
uint8_t buf_rx_param[255]
static void sts3032_event(struct sts3032 *sts)
void sts3032_set_response_level(struct sts3032 *sts, uint8_t id, uint8_t level)
uint16_t pos[SERVOS_STS3032_NB]
struct uart_periph * periph
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len)
Copy buf in the circular buffer.
void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len)
initialize a circular buffer.
int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len)
copy the next buffer available in cb to buf.
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
uint8_t uart_getch(struct uart_periph *p)
#define SMS_STS_RESPONSE_LEVEL
#define SMS_STS_TORQUE_ENABLE
#define SMS_STS_GOAL_POSITION_L
#define SMS_STS_PRESENT_POSITION_L
Architecture independent timing functions.
Periodic telemetry system header (includes downlink utility and generated code).
void WEAK uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static const float offset[]
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
enum navigation_state current_state