Created generic SmartAudio interface (#5561)
This commit is contained in:
codecae
Michael Keller
parent
cf6b3639e2
commit
8131e4f840
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
Loading…
Reference in New Issue