From 8131e4f840ba94e7bb1481ee0578b99475517f51 Mon Sep 17 00:00:00 2001 From: codecae Date: Fri, 30 Mar 2018 20:01:30 -0400 Subject: [PATCH] Created generic SmartAudio interface (#5561) --- make/source.mk | 1 + src/main/interface/smartaudio_protocol.c | 197 +++++++++++++++++++++++ src/main/interface/smartaudio_protocol.h | 98 +++++++++++ 3 files changed, 296 insertions(+) create mode 100644 src/main/interface/smartaudio_protocol.c create mode 100644 src/main/interface/smartaudio_protocol.h diff --git a/make/source.mk b/make/source.mk index 2efbcd2c7..185dd5339 100644 --- a/make/source.mk +++ b/make/source.mk @@ -59,6 +59,7 @@ COMMON_SRC = \ interface/msp.c \ interface/msp_box.c \ interface/tramp_protocol.c \ + interface/smartaudio_protocol.c \ io/beeper.c \ io/piniobox.c \ io/serial.c \ diff --git a/src/main/interface/smartaudio_protocol.c b/src/main/interface/smartaudio_protocol.c new file mode 100644 index 000000000..d20493039 --- /dev/null +++ b/src/main/interface/smartaudio_protocol.c @@ -0,0 +1,197 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include "platform.h" +#include "common/crc.h" +#include "interface/smartaudio_protocol.h" + +#define SMARTAUDIO_SYNC_BYTE 0xAA +#define SMARTAUDIO_HEADER_BYTE 0x55 +#define SMARTAUDIO_START_CODE SMARTAUDIO_SYNC_BYTE + SMARTAUDIO_HEADER_BYTE +#define SMARTAUDIO_GET_PITMODE_FREQ (1 << 14) +#define SMARTAUDIO_SET_PITMODE_FREQ (1 << 15) +#define SMARTAUDIO_FREQUENCY_MASK 0x3FFF + +#define SMARTAUDIO_CMD_GET_SETTINGS 0x03 +#define SMARTAUDIO_CMD_SET_POWER 0x05 +#define SMARTAUDIO_CMD_SET_CHANNEL 0x07 +#define SMARTAUDIO_CMD_SET_FREQUENCY 0x09 +#define SMARTAUDIO_CMD_SET_MODE 0x0B + +#define SMARTAUDIO_RSP_GET_SETTINGS_V1 SMARTAUDIO_CMD_GET_SETTINGS >> 1 +#define SMARTAUDIO_RSP_GET_SETTINGS_V2 (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x08 +#define SMARTAUDIO_RSP_SET_POWER SMARTAUDIO_CMD_SET_POWER >> 1 +#define SMARTAUDIO_RSP_SET_CHANNEL SMARTAUDIO_CMD_SET_CHANNEL >> 1 +#define SMARTAUDIO_RSP_SET_FREQUENCY SMARTAUDIO_CMD_SET_FREQUENCY >> 1 +#define SMARTAUDIO_RSP_SET_MODE SMARTAUDIO_CMD_SET_MODE >> 1 + +#define SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel) (band * 8 + (channel)) +#define U16BIGENDIAN(bytes) (bytes << 8) | ((bytes >> 8) & 0xFF) + +static void smartaudioFrameInit(const uint8_t command, smartaudioFrameHeader_t *header, const uint8_t payloadLength) +{ + header->startCode = SMARTAUDIO_START_CODE; + header->length = payloadLength; + header->command = command; +} + +static void smartaudioUnpackOperationMode(smartaudioSettings_t *settings, const uint8_t operationMode, const bool settingsResponse) +{ + if (settingsResponse) { + // operation mode bit order is different between 'Get Settings' and 'Set Mode' responses. + settings->userFrequencyMode = operationMode & 0x01; + settings->pitmodeEnabled = operationMode & 0x02; + settings->pitmodeInRangeActive = operationMode & 0x04; + settings->pitmodeOutRangeActive = operationMode & 0x08; + settings->unlocked = operationMode & 0x10; + } else { + settings->pitmodeInRangeActive = operationMode & 0x01; + settings->pitmodeOutRangeActive = operationMode & 0x02; + settings->pitmodeEnabled = operationMode & 0x04; + settings->unlocked = operationMode & 0x08; + } +} + +static void smartaudioUnpackFrequency(smartaudioSettings_t *settings, const uint16_t frequency) +{ + if (frequency & SMARTAUDIO_GET_PITMODE_FREQ) { + settings->pitmodeFrequency = U16BIGENDIAN(frequency & SMARTAUDIO_FREQUENCY_MASK); + } else { + settings->frequency = U16BIGENDIAN(frequency & SMARTAUDIO_FREQUENCY_MASK); + } +} + +static void smartaudioUnpackSettings(smartaudioSettings_t *settings, const smartaudioSettingsResponseFrame_t *frame) +{ + settings->channel = frame->channel; + settings->power = frame->power; + smartaudioUnpackFrequency(settings, frame->frequency); + smartaudioUnpackOperationMode(settings, frame->operationMode, true); +} + +static uint8_t smartaudioPackOperationMode(const smartaudioSettings_t *settings) +{ + uint8_t operationMode = 0; + operationMode |= settings->pitmodeInRangeActive << 0; + operationMode |= settings->pitmodeOutRangeActive << 1; + operationMode |= settings->pitmodeEnabled << 2; + operationMode |= settings->unlocked << 3; + return operationMode; +} + +size_t smartaudioFrameGetSettings(smartaudioFrame_t *smartaudioFrame) +{ + smartaudioCommandOnlyFrame_t *frame = (smartaudioCommandOnlyFrame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_GET_SETTINGS, &frame->header, 0); + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioCommandOnlyFrame_t) - sizeof(frame->crc)); + return sizeof(smartaudioCommandOnlyFrame_t); +} + +size_t smartaudioFrameGetPitmodeFrequency(smartaudioFrame_t *smartaudioFrame) +{ + smartaudioU16Frame_t *frame = (smartaudioU16Frame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_SET_FREQUENCY, &frame->header, sizeof(frame->payload)); + frame->payload = SMARTAUDIO_SET_PITMODE_FREQ; + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU16Frame_t) - sizeof(frame->crc)); + return sizeof(smartaudioU16Frame_t); +} + +size_t smartaudioFrameSetPower(smartaudioFrame_t *smartaudioFrame, const uint8_t power) +{ + smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_SET_POWER, &frame->header, sizeof(frame->payload)); + frame->payload = power; + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc)); + return sizeof(smartaudioU8Frame_t); +} + +size_t smartaudioFrameSetBandChannel(smartaudioFrame_t *smartaudioFrame, const uint8_t band, const uint8_t channel) +{ + smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_SET_CHANNEL, &frame->header, sizeof(frame->payload)); + frame->payload = SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel); + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc)); + return sizeof(smartaudioU8Frame_t); +} + +size_t smartaudioFrameSetFrequency(smartaudioFrame_t *smartaudioFrame, const uint16_t frequency, const bool pitmodeFrequency) +{ + smartaudioU16Frame_t *frame = (smartaudioU16Frame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_SET_FREQUENCY, &frame->header, sizeof(frame->payload)); + frame->payload = U16BIGENDIAN(frequency | (pitmodeFrequency ? SMARTAUDIO_SET_PITMODE_FREQ : 0x00)); + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU16Frame_t) - sizeof(frame->crc)); + return sizeof(smartaudioU16Frame_t); +} + +size_t smartaudioFrameSetOperationMode(smartaudioFrame_t *smartaudioFrame, const smartaudioSettings_t *settings) +{ + smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame; + smartaudioFrameInit(SMARTAUDIO_CMD_SET_MODE, &frame->header, sizeof(frame->payload)); + frame->payload = smartaudioPackOperationMode(settings); + frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc)); + return sizeof(smartaudioU8Frame_t); +} + +bool smartaudioParseResponseBuffer(smartaudioSettings_t *settings, const uint8_t *buffer) +{ + const smartaudioFrameHeader_t *header = (const smartaudioFrameHeader_t *)buffer; + const uint8_t fullFrameLength = sizeof(smartaudioFrameHeader_t) + header->length; + const uint8_t headerPayloadLength = fullFrameLength - 1; // subtract crc byte from length + const uint8_t *endPtr = buffer + fullFrameLength; + + if (crc8_dvb_s2_update(*endPtr, buffer, headerPayloadLength) || header->startCode != SMARTAUDIO_START_CODE) { + return false; + } + switch (header->command) { + case SMARTAUDIO_RSP_GET_SETTINGS_V1: { + const smartaudioSettingsResponseFrame_t *resp = (const smartaudioSettingsResponseFrame_t *)buffer; + settings->version = 1; + smartaudioUnpackSettings(settings, resp); + } + break; + case SMARTAUDIO_RSP_GET_SETTINGS_V2: { + const smartaudioSettingsResponseFrame_t *resp = (const smartaudioSettingsResponseFrame_t *)buffer; + settings->version = 2; + smartaudioUnpackSettings(settings, resp); + } + break; + case SMARTAUDIO_RSP_SET_POWER: { + const smartaudioU16ResponseFrame_t *resp = (const smartaudioU16ResponseFrame_t *)buffer; + settings->channel = (resp->payload >> 8) & 0xFF; + settings->power = resp->payload & 0xFF; + } + break; + case SMARTAUDIO_RSP_SET_CHANNEL: { + const smartaudioU8ResponseFrame_t *resp = (const smartaudioU8ResponseFrame_t *)buffer; + settings->channel = resp->payload; + } + break; + case SMARTAUDIO_RSP_SET_FREQUENCY: { + const smartaudioU16ResponseFrame_t *resp = (const smartaudioU16ResponseFrame_t *)buffer; + smartaudioUnpackFrequency(settings, resp->payload); + } + break; + case SMARTAUDIO_RSP_SET_MODE: { + const smartaudioU8ResponseFrame_t *resp = (const smartaudioU8ResponseFrame_t*)buffer; + smartaudioUnpackOperationMode(settings, resp->payload, false); + } + break; + default: + return false; + } + return true; +} diff --git a/src/main/interface/smartaudio_protocol.h b/src/main/interface/smartaudio_protocol.h new file mode 100644 index 000000000..b532dc19b --- /dev/null +++ b/src/main/interface/smartaudio_protocol.h @@ -0,0 +1,98 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "drivers/serial.h" + +#define SMARTAUDIO_SERIAL_OPTIONS SERIAL_NOT_INVERTED | SERIAL_BIDIR_NOPULL | SERIAL_STOPBITS_2 +#define SMARTAUDIO_DEFAULT_BAUD 4900 +#define SMARTAUDIO_MIN_BAUD 4800 +#define SMARTAUDIO_MAX_BAUD 4950 + +typedef struct smartaudioSettings_s { + uint8_t version; + uint8_t unlocked; + uint8_t channel; + uint8_t power; + uint16_t frequency; + uint16_t pitmodeFrequency; + bool userFrequencyMode; + bool pitmodeEnabled; + bool pitmodeInRangeActive; + bool pitmodeOutRangeActive; +} smartaudioSettings_t; + +typedef struct smartaudioFrameHeader_s { + uint16_t startCode; + uint8_t length; + uint8_t command; +} __attribute__((packed)) smartaudioFrameHeader_t; + +typedef struct smartaudioCommandOnlyFrame_s { + smartaudioFrameHeader_t header; + uint8_t crc; +} __attribute__((packed)) smartaudioCommandOnlyFrame_t; + +typedef struct smartaudioU8Frame_s { + smartaudioFrameHeader_t header; + uint8_t payload; + uint8_t crc; +} __attribute__((packed)) smartaudioU8Frame_t; + +typedef struct smartaudioU16Frame_s { + smartaudioFrameHeader_t header; + uint16_t payload; + uint8_t crc; +} __attribute__((packed)) smartaudioU16Frame_t; + +typedef struct smartaudioU8ResponseFrame_s { + smartaudioFrameHeader_t header; + uint8_t payload; + uint8_t reserved; + uint8_t crc; +} __attribute__((packed)) smartaudioU8ResponseFrame_t; + +typedef struct smartaudioU16ResponseFrame_s { + smartaudioFrameHeader_t header; + uint16_t payload; + uint8_t reserved; + uint8_t crc; +} __attribute__((packed)) smartaudioU16ResponseFrame_t; + +typedef struct smartaudioSettingsResponseFrame_s { + smartaudioFrameHeader_t header; + uint8_t channel; + uint8_t power; + uint8_t operationMode; + uint16_t frequency; + uint8_t crc; +} __attribute__((packed)) smartaudioSettingsResponseFrame_t; + +typedef union smartaudioFrame_u { + smartaudioCommandOnlyFrame_t commandOnlyFrame; + smartaudioU8Frame_t u8RequestFrame; + smartaudioU16Frame_t u16RequestFrame; +} __attribute__((packed)) smartaudioFrame_t; + +size_t smartaudioFrameGetSettings(smartaudioFrame_t *smartaudioFrame); +size_t smartaudioFrameGetPitmodeFrequency(smartaudioFrame_t *smartaudioFrame); +size_t smartaudioFrameSetPower(smartaudioFrame_t *smartaudioFrame, const uint8_t power); +size_t smartaudioFrameSetBandChannel(smartaudioFrame_t *smartaudioFrame, const uint8_t band, const uint8_t channel); +size_t smartaudioFrameSetFrequency(smartaudioFrame_t *smartaudioFrame, const uint16_t frequency, const bool pitmodeFrequency); +size_t smartaudioFrameSetOperationMode(smartaudioFrame_t *smartaudioFrame, const smartaudioSettings_t *settings); +bool smartaudioParseResponseBuffer(smartaudioSettings_t *settings, const uint8_t *buffer);