158 lines
4.0 KiB
C
158 lines
4.0 KiB
C
#include "protocol.h"
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "drive_control.h"
|
|
|
|
static volatile bool s_handshake_verified = false;
|
|
|
|
|
|
static bool parse_drive_mode(const char *value, drive_mode_t *mode, const char **next)
|
|
{
|
|
if (strncmp(value, "ECO:", 4) == 0) {
|
|
*mode = DRIVE_MODE_ECO;
|
|
*next = value + 4;
|
|
return true;
|
|
}
|
|
if (strncmp(value, "NORMAL:", 7) == 0) {
|
|
*mode = DRIVE_MODE_NORMAL;
|
|
*next = value + 7;
|
|
return true;
|
|
}
|
|
if (strncmp(value, "BOOST:", 6) == 0) {
|
|
*mode = DRIVE_MODE_BOOST;
|
|
*next = value + 6;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static bool parse_duty_value(const char *value, uint32_t *result)
|
|
{
|
|
char *end = NULL;
|
|
const unsigned long parsed = strtoul(value, &end, 10);
|
|
if (end == value || *end != '\0') {
|
|
return false;
|
|
}
|
|
if (parsed > 1023UL) {
|
|
return false;
|
|
}
|
|
*result = (uint32_t)parsed;
|
|
return true;
|
|
}
|
|
|
|
void protocol_reset_session(void)
|
|
{
|
|
s_handshake_verified = false;
|
|
}
|
|
|
|
bool protocol_is_handshake_verified(void)
|
|
{
|
|
return s_handshake_verified;
|
|
}
|
|
|
|
bool protocol_build_response(const char *input, char *output, size_t output_size)
|
|
{
|
|
if (input == NULL || output == NULL || output_size == 0) {
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "HELLO") == 0) {
|
|
s_handshake_verified = true;
|
|
drive_control_set_client_connected(true);
|
|
snprintf(output, output_size, "HELLO:SKATE_V1\n");
|
|
return true;
|
|
}
|
|
|
|
if (strncmp(input, "PING:", 5) == 0) {
|
|
snprintf(output, output_size, "PONG:%s\n", input + 5);
|
|
return true;
|
|
}
|
|
|
|
// Emergency stop is intentionally allowed before handshake.
|
|
// It only moves the system toward a safer motor state.
|
|
if (strcmp(input, "ESTOP") == 0) {
|
|
drive_control_trigger_estop();
|
|
return false;
|
|
}
|
|
|
|
if (!s_handshake_verified) {
|
|
snprintf(output, output_size, "ERR:HANDSHAKE_REQUIRED\n");
|
|
return true;
|
|
}
|
|
|
|
if (strcmp(input, "THR:PRESS") == 0) {
|
|
drive_control_handle_throttle_press();
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "THR:RELEASE") == 0) {
|
|
drive_control_handle_throttle_release();
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "BRK:PRESS") == 0) {
|
|
drive_control_handle_brake_press();
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "BRK:RELEASE") == 0) {
|
|
drive_control_handle_brake_release();
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "MODE:ECO") == 0) {
|
|
drive_control_set_mode(DRIVE_MODE_ECO);
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "MODE:NORMAL") == 0) {
|
|
drive_control_set_mode(DRIVE_MODE_NORMAL);
|
|
return false;
|
|
}
|
|
|
|
if (strcmp(input, "MODE:BOOST") == 0) {
|
|
drive_control_set_mode(DRIVE_MODE_BOOST);
|
|
return false;
|
|
}
|
|
|
|
if (strncmp(input, "CAL:", 4) == 0) {
|
|
drive_mode_t mode = DRIVE_MODE_NORMAL;
|
|
const char *setting = NULL;
|
|
if (!parse_drive_mode(input + 4, &mode, &setting)) {
|
|
snprintf(output, output_size, "ERR:CAL_MODE\n");
|
|
return true;
|
|
}
|
|
|
|
if (strncmp(setting, "MIN=", 4) == 0) {
|
|
uint32_t value = 0U;
|
|
if (!parse_duty_value(setting + 4, &value)) {
|
|
snprintf(output, output_size, "ERR:CAL_VALUE\n");
|
|
return true;
|
|
}
|
|
drive_control_set_mode_pwm_min_duty(mode, value);
|
|
snprintf(output, output_size, "OK:CAL\n");
|
|
return true;
|
|
}
|
|
|
|
if (strncmp(setting, "MAX=", 4) == 0) {
|
|
uint32_t value = 0U;
|
|
if (!parse_duty_value(setting + 4, &value)) {
|
|
snprintf(output, output_size, "ERR:CAL_VALUE\n");
|
|
return true;
|
|
}
|
|
drive_control_set_mode_pwm_max_duty(mode, value);
|
|
snprintf(output, output_size, "OK:CAL\n");
|
|
return true;
|
|
}
|
|
|
|
snprintf(output, output_size, "ERR:CAL_FIELD\n");
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|