Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Merge pull request #10214 from azolyoung/add_fc_attitude_for_rcdevice

Browse source
This commit is contained in:
Michael Keller 2020-10-01 23:57:52 +13:00 • committed by GitHub
commit 5db852a2f4
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 147 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

115
src/main/io/rcdevice.c
View file

@ -30,6 +30,8 @@
#include "drivers/time.h"
#include "flight/imu.h"
#include "io/serial.h"
#include "pg/rcdevice.h"
@ -38,6 +40,24 @@
#ifdef USE_RCDEVICE
typedef enum {
RCDEVICE_STATE_WAITING_HEADER = 0,
RCDEVICE_STATE_WAITING_COMMAND,
RCDEVICE_STATE_WAITING_DATA_LENGTH,
RCDEVICE_STATE_WAITING_DATA,
RCDEVICE_STATE_WAITING_CRC,
} RCDEVICE_PARSER_STATE;
typedef struct {
uint8_t state;
uint8_t expectedDataLength;
runcamDeviceRequest_t request;
timeUs_t lastRecvDataTimestamp;
uint8_t crc;
uint8_t isParseDone;
} runcamDeviceRequestParseContext_t;
typedef struct runcamDeviceExpectedResponseLength_s {
uint8_t command;
uint8_t reponseLength;
@ -52,6 +72,8 @@ static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = {
rcdeviceWaitingResponseQueue waitingResponseQueue;
static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer
static runcamDeviceRequestParseContext_t requestParserContext;
static runcamDevice_t *currentDevice;
static uint8_t runcamDeviceGetRespLen(uint8_t command)
{
@ -280,6 +302,8 @@ void runcamDeviceInit(runcamDevice_t *device)
runcamDeviceGetDeviceInfo(device);
}
}
currentDevice = device;
}
bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
@ -358,10 +382,22 @@ static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs
return respCtx;
}
runcamDeviceRequest_t* rcdeviceGetRequest()
{
if (requestParserContext.isParseDone) {
// reset the parse done state, then we can handle next request from rcdevice
requestParserContext.isParseDone = 0;
return &requestParserContext.request;
}
return NULL;
}
void rcdeviceReceive(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
// process the requests trigger by FC
rcdeviceResponseParseContext_t *respCtx = NULL;
while ((respCtx = getWaitingResponse(millis())) != NULL) {
if (!serialRxBytesWaiting(respCtx->device->serialPort)) {
@ -409,6 +445,85 @@ void rcdeviceReceive(timeUs_t currentTimeUs)
}
}
}
// process the requests trigger by device
while (currentDevice != NULL) {
if (!serialRxBytesWaiting(currentDevice->serialPort)) {
break;
}
// if lastest packet still not handled, do parse next bytes.
if (requestParserContext.isParseDone) {
break;
}
// if it is during the packet receiving progress, check if it is already timeout(200 ms),
// if timeout, then reset the state, else the later requests can't be accept
if (requestParserContext.state != RCDEVICE_STATE_WAITING_HEADER && millis() - requestParserContext.lastRecvDataTimestamp > 200) {
memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t));
requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND; // reset state to waiting header
}
const uint8_t c = serialRead(currentDevice->serialPort);
switch (requestParserContext.state) {
case RCDEVICE_STATE_WAITING_HEADER:
if (c == RCDEVICE_PROTOCOL_HEADER) {
memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t));
requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND;
}
break;
case RCDEVICE_STATE_WAITING_COMMAND:
requestParserContext.request.command = c;
// there is no payload for RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, skip to waiting crc step
if (requestParserContext.request.command == RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE) {
requestParserContext.state = RCDEVICE_STATE_WAITING_CRC;
} else {
// for now, only RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE support, so reset the state to waiting header.
requestParserContext.state = RCDEVICE_PROTOCOL_HEADER;
}
break;
case RCDEVICE_STATE_WAITING_DATA_LENGTH:
requestParserContext.expectedDataLength = c;
requestParserContext.state = RCDEVICE_STATE_WAITING_DATA;
break;
case RCDEVICE_STATE_WAITING_DATA:
if (requestParserContext.request.dataLength < requestParserContext.expectedDataLength) {
requestParserContext.request.data[requestParserContext.request.dataLength] = c;
requestParserContext.request.dataLength++;
}
if (requestParserContext.request.dataLength == requestParserContext.expectedDataLength) {
requestParserContext.state = RCDEVICE_STATE_WAITING_CRC; // data received done
}
break;
case RCDEVICE_STATE_WAITING_CRC: {
// verify crc
uint8_t crc = 0;
uint8_t header = RCDEVICE_PROTOCOL_HEADER;
crc = crc8_dvb_s2_update(crc, &header, 1);
crc = crc8_dvb_s2_update(crc, &requestParserContext.request.command, 1);
crc = crc8_dvb_s2_update(crc, &requestParserContext.request.data, requestParserContext.request.dataLength);
if (crc == c) {
requestParserContext.isParseDone = 1;
}
requestParserContext.state = RCDEVICE_STATE_WAITING_HEADER;
}
break;
}
requestParserContext.lastRecvDataTimestamp = millis();
}
}
void runcamDeviceSendAttitude(runcamDevice_t *device)
{
uint16_t buf[3];
buf[0] = attitude.values.roll;
buf[1] = attitude.values.pitch;
buf[2] = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, (uint8_t *)buf, sizeof(buf));
}
#endif

14
src/main/io/rcdevice.h
View file

@ -38,6 +38,7 @@
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS 0x02
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04
#define RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE 0x50
// Old protocol defines
#define RCSPLIT_PACKET_HEADER 0x55
@ -53,6 +54,7 @@ typedef enum {
RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6),
RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7),
RCDEVICE_PROTOCOL_FEATURE_CMS_MENU = (1 << 8),
RCDEVICE_PROTOCOL_FEATURE_FC_ATTITUDE = (1 << 9)
} rcdevice_features_e;
// Operation of Camera Button Simulation
@ -150,6 +152,12 @@ typedef struct {
rcdeviceRespParseFunc parseFunc;
} rcdeviceWaitingResponseQueue;
typedef struct {
uint8_t command;
uint8_t data[RCDEVICE_PROTOCOL_MAX_DATA_SIZE - 1];
uint8_t dataLength;
} runcamDeviceRequest_t;
void runcamDeviceInit(runcamDevice_t *device);
void rcdeviceReceive(timeUs_t currentTimeUs);
@ -160,4 +168,8 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc);
void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
void runcamDeviceSendAttitude(runcamDevice_t *device);
runcamDeviceRequest_t* rcdeviceGetRequest();

18
src/main/io/rcdevice_cam.c
View file

@ -55,7 +55,7 @@ bool isButtonPressed = false;
bool waitingDeviceResponse = false;
static bool isFeatureSupported(uint8_t feature)
static bool isFeatureSupported(uint16_t feature)
{
if (camDevice->info.features & feature || rcdeviceConfig()->feature & feature) {
return true;
@ -284,6 +284,15 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
}
}
static void rcdeviceProcessDeviceRequest(runcamDeviceRequest_t *request)
{
switch (request->command) {
case RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE:
runcamDeviceSendAttitude(camDevice);
break;
}
}
void rcdeviceUpdate(timeUs_t currentTimeUs)
{
rcdeviceReceive(currentTimeUs);
@ -291,6 +300,13 @@ void rcdeviceUpdate(timeUs_t currentTimeUs)
rcdeviceCameraControlProcess();
rcdevice5KeySimulationProcess(currentTimeUs);
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_FC_ATTITUDE)) {
runcamDeviceRequest_t *request = rcdeviceGetRequest();
if (request) {
rcdeviceProcessDeviceRequest(request);
}
}
}
void rcdeviceInit(void)

2
src/test/unit/rcdevice_unittest.cc
View file

@ -32,6 +32,7 @@ extern "C" {
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "flight/imu.h"
#include "drivers/serial.h"
@ -995,4 +996,5 @@ extern "C" {
uint32_t resumeRefreshAt = 0;
int getArmingDisableFlags(void) {return 0;}
void pinioBoxTaskControl(void) {}
attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
}