Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin' into dzikuvx-pcf8574-expander

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-07-20 22:12:40 +02:00
commit 1547aff0aa
16 changed files with 866 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

3
docs/Global Functions.md
View file

@ -27,7 +27,8 @@ _Global Functions_ (abbr. GF) are a mechanism allowing to override certain fligh
| 6 | INVERT_YAW | Inverts YAW axis input for PID/PIFF controller |
| 7 | OVERRIDE_THROTTLE | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
| 8 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` |
| 8 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
| 9 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
| 10 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` |
## Flags

1
make/source.mk
View file

@ -204,6 +204,7 @@ COMMON_SRC = \
io/osd_common.c \
io/osd_grid.c \
io/osd_hud.c \
io/smartport_master.c \
navigation/navigation.c \
navigation/navigation_fixedwing.c \
navigation/navigation_fw_launch.c \

3
src/main/build/debug.h
View file

@ -73,6 +73,9 @@ typedef enum {
DEBUG_IRLOCK,
DEBUG_CD,
DEBUG_KALMAN,
DEBUG_SPM_CELLS, // Smartport master FLVSS
DEBUG_SPM_VS600, // Smartport master VS600 VTX
DEBUG_SPM_VARIO, // Smartport master variometer
DEBUG_PCF8574,
DEBUG_COUNT
} debugType_e;

3
src/main/common/utils.h
View file

@ -52,6 +52,7 @@
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
#define BIT(x) (1 << (x))
#define GET_BIT(value, bit) ((value >> bit) & 1)
#define STATIC_ASSERT(condition, name) \
typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused))
@ -112,3 +113,5 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
#define UNREACHABLE() __builtin_unreachable()
#define ALIGNED(x) __attribute__ ((aligned(x)))
#define PACKED __attribute__((packed))

3
src/main/config/parameter_group_ids.h
View file

@ -111,7 +111,8 @@
#define PG_ESC_SENSOR_CONFIG 1021
#define PG_RPM_FILTER_CONFIG 1022
#define PG_GLOBAL_VARIABLE_CONFIG 1023
#define PG_INAV_END 1023
#define PG_SMARTPORT_MASTER_CONFIG 1024
#define PG_INAV_END 1024
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

5
src/main/fc/fc_init.c
View file

@ -115,6 +115,7 @@
#include "io/rcdevice_cam.h"
#include "io/serial.h"
#include "io/displayport_msp.h"
#include "io/smartport_master.h"
#include "io/vtx.h"
#include "io/vtx_control.h"
#include "io/vtx_smartaudio.h"
@ -281,6 +282,10 @@ void init(void)
djiOsdSerialInit();
#endif
#if defined(USE_SMARTPORT_MASTER)
smartportMasterInit();
#endif
#if defined(USE_LOG)
// LOG might use serial output, so we only can init it after serial port is ready
// From this point on we can use LOG_*() to produce real-time debugging information

20
src/main/fc/fc_tasks.c
View file

@ -61,6 +61,7 @@
#include "io/pwmdriver_i2c.h"
#include "io/serial.h"
#include "io/rcdevice_cam.h"
#include "io/smartport_master.h"
#include "io/vtx.h"
#include "io/osd_dji_hd.h"
#include "io/servo_sbus.h"
@ -251,6 +252,13 @@ void taskTelemetry(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_SMARTPORT_MASTER)
void taskSmartportMaster(timeUs_t currentTimeUs)
{
smartportMasterHandle(currentTimeUs);
}
#endif
#ifdef USE_LED_STRIP
void taskLedStrip(timeUs_t currentTimeUs)
{
@ -360,6 +368,9 @@ void fcTasksInit(void)
#ifdef USE_IRLOCK
setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
#endif
#if defined(USE_SMARTPORT_MASTER)
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
#endif
}
cfTask_t cfTasks[TASK_COUNT] = {
@ -494,6 +505,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#if defined(USE_SMARTPORT_MASTER)
[TASK_SMARTPORT_MASTER] = {
.taskName = "SPORT MASTER",
.taskFunc = taskSmartportMaster,
.desiredPeriod = TASK_PERIOD_HZ(500), // 500 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_LED_STRIP
[TASK_LEDSTRIP] = {
.taskName = "LEDSTRIP",

16
src/main/fc/settings.yaml
View file

@ -25,7 +25,7 @@ tables:
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST"]
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "FPORT2", "SBUS_FAST"]
- name: rx_spi_protocol
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
enum: rx_spi_protocol_e
@ -84,7 +84,7 @@ tables:
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN", "PCF8574"]
"IRLOCK", "CD", "KALMAN", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -3005,3 +3005,15 @@ groups:
- name: esc_sensor_listen_only
field: listenOnly
type: bool
- name: PG_SMARTPORT_MASTER_CONFIG
type: smartportMasterConfig_t
headers: ["io/smartport_master.h"]
condition: USE_SMARTPORT_MASTER
members:
- name: smartport_master_halfduplex
field: halfDuplex
type: bool
- name: smartport_master_inverted
field: inverted
type: bool

6
src/main/io/osd.c
View file

@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED
#include "common/time.h"
#include "common/typeconversion.h"
#include "common/utils.h"
#include "programming/global_functions.h"
#include "config/feature.h"
#include "config/parameter_group.h"
@ -3241,6 +3242,11 @@ void osdUpdate(timeUs_t currentTimeUs)
if (IS_RC_MODE_ACTIVE(BOXOSDALT1))
activeLayout = 1;
else
#ifdef USE_PROGRAMMING_FRAMEWORK
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT))
activeLayout = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT);
else
#endif
activeLayout = 0;
}
if (currentLayout != activeLayout) {

1
src/main/io/serial.h
View file

@ -54,6 +54,7 @@ typedef enum {
FUNCTION_FRSKY_OSD = (1 << 20), // 1048576
FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152
FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304
FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
} serialPortFunction_e;
typedef enum {

591
src/main/io/smartport_master.c Normal file
View file

@ -0,0 +1,591 @@
/*
* This file is part of iNav.
*
* iNav 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.
*
* iNav 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 iNav. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
FILE_COMPILE_FOR_SPEED
#if defined(USE_SMARTPORT_MASTER)
#include "build/debug.h"
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "io/serial.h"
#include "io/smartport_master.h"
#include "rx/frsky_crc.h"
enum {
PRIM_DISCARD_FRAME = 0x00,
PRIM_DATA_FRAME = 0x10,
PRIM_WORKING_STATE = 0x20,
PRIM_IDLE_STATE = 0x21,
PRIM_CONFIG_READ = 0x30,
PRIM_CONFIG_WRITE = 0x31,
PRIM_CONFIG_RESPONSE = 0x32,
PRIM_DIAG_READ = 0X40,
PRIM_DIAG_WRITE = 0X41,
PRIM_ENABLE_APP_NODE = 0x70,
PRIM_DISABLE_APP_NODE = 0x71,
};
enum
{
DATAID_SPEED = 0x0830,
DATAID_VFAS = 0x0210,
DATAID_CURRENT = 0x0200,
DATAID_RPM = 0x050F,
DATAID_ALTITUDE = 0x0100,
DATAID_FUEL = 0x0600,
DATAID_ADC1 = 0xF102,
DATAID_ADC2 = 0xF103,
DATAID_LATLONG = 0x0800,
DATAID_CAP_USED = 0x0600,
DATAID_VARIO = 0x0110,
DATAID_CELLS = 0x0300,
DATAID_CELLS_LAST = 0x030F,
DATAID_HEADING = 0x0840,
DATAID_FPV = 0x0450,
DATAID_PITCH = 0x0430,
DATAID_ROLL = 0x0440,
DATAID_ACCX = 0x0700,
DATAID_ACCY = 0x0710,
DATAID_ACCZ = 0x0720,
DATAID_T1 = 0x0400,
DATAID_T2 = 0x0410,
DATAID_HOME_DIST = 0x0420,
DATAID_GPS_ALT = 0x0820,
DATAID_ASPD = 0x0A00,
DATAID_A3 = 0x0900,
DATAID_A4 = 0x0910,
DATAID_VS600 = 0x0E10
};
#define SMARTPORT_BAUD 57600
#define SMARTPORT_UART_MODE MODE_RXTX
#define SMARTPORT_PHYID_MAX 0x1B
#define SMARTPORT_PHYID_COUNT (SMARTPORT_PHYID_MAX + 1)
#define SMARTPORT_POLLING_INTERVAL 12 // ms
#define SMARTPORT_FRAME_START 0x7E
#define SMARTPORT_BYTESTUFFING_MARKER 0x7D
#define SMARTPORT_BYTESTUFFING_XOR_VALUE 0x20
#define SMARTPORT_SENSOR_DATA_TIMEOUT 500 // ms
#define SMARTPORT_FORWARD_REQUESTS_MAX 10
typedef enum {
PT_ACTIVE_ID,
PT_INACTIVE_ID
} pollType_e;
typedef struct smartPortMasterFrame_s {
uint8_t magic;
uint8_t phyID;
smartPortPayload_t payload;
uint8_t crc;
} PACKED smartportFrame_t;
typedef union {
smartportFrame_t frame;
uint8_t bytes[sizeof(smartportFrame_t)];
} smartportFrameBuffer_u;
typedef struct {
cellsData_t cells;
timeUs_t cellsTimestamp;
vs600Data_t vs600;
timeUs_t vs600Timestamp;
int32_t altitude;
timeUs_t altitudeTimestamp;
int16_t vario;
timeUs_t varioTimestamp;
} smartportSensorsData_t;
typedef struct {
uint8_t phyID;
smartPortPayload_t payload;
} smartportForwardData_t;
PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0);
PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig,
.halfDuplex = true,
.inverted = false
);
static serialPort_t *smartportMasterSerialPort = NULL;
static serialPortConfig_t *portConfig;
static int8_t currentPolledPhyID = -1;
static int8_t forcedPolledPhyID = -1;
static uint8_t rxBufferLen = 0;
static uint32_t activePhyIDs = 0;
static smartPortPayload_t sensorPayloadCache[SMARTPORT_PHYID_COUNT];
static uint8_t forwardRequestsStart = 0;
static uint8_t forwardRequestsEnd = 0;
static smartportForwardData_t forwardRequests[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward requests' circular buffer
static uint8_t forwardResponsesCount = 0;
static smartportForwardData_t forwardResponses[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward responses' buffer
static smartportSensorsData_t sensorsData;
bool smartportMasterInit(void)
{
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT_MASTER);
if (!portConfig) {
return false;
}
portOptions_t portOptions = (smartportMasterConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (smartportMasterConfig()->inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
smartportMasterSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT_MASTER, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
memset(&sensorsData, 0, sizeof(sensorsData));
return true;
}
static void phyIDSetActive(uint8_t phyID, bool active)
{
uint32_t mask = 1 << phyID;
if (active) {
activePhyIDs |= mask;
} else {
activePhyIDs &= ~mask;
}
}
static uint32_t phyIDAllActiveMask(void)
{
uint32_t mask = 0;
for (uint8_t i = 0; i < SMARTPORT_PHYID_COUNT; ++i) {
mask |= 1 << i;
}
return mask;
}
static int8_t phyIDNext(uint8_t start, bool active, bool loop)
{
for (uint8_t i = start; i < ((loop ? start : 0) + SMARTPORT_PHYID_COUNT); ++i) {
uint8_t phyID = i % SMARTPORT_PHYID_COUNT;
uint32_t mask = 1 << phyID;
uint32_t phyIDMasked = activePhyIDs & mask;
if ((active && phyIDMasked) || !(active || phyIDMasked)) {
return phyID;
}
}
return -1;
}
static bool phyIDIsActive(uint8_t id)
{
return !!(activePhyIDs & (1 << id));
}
static bool phyIDNoneActive(void)
{
return activePhyIDs == 0;
}
static bool phyIDAllActive(void)
{
static uint32_t allActiveMask = 0;
if (!allActiveMask) {
allActiveMask = phyIDAllActiveMask();
}
return !!((activePhyIDs & allActiveMask) == allActiveMask);
}
static bool phyIDAnyActive(void)
{
return !!activePhyIDs;
}
static void smartportMasterSendByte(uint8_t byte)
{
serialWrite(smartportMasterSerialPort, byte);
}
static void smartportMasterPhyIDFillCheckBits(uint8_t *phyIDByte)
{
*phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 1) ^ GET_BIT(*phyIDByte, 2)) << 5;
*phyIDByte |= (GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 3) ^ GET_BIT(*phyIDByte, 4)) << 6;
*phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 4)) << 7;
}
static void smartportMasterPoll(void)
{
static pollType_e nextPollType = PT_INACTIVE_ID;
static uint8_t nextActivePhyID = 0, nextInactivePhyID = 0;
uint8_t phyIDToPoll;
if (currentPolledPhyID != -1) {
// currentPolledPhyID hasn't been reset by smartportMasterReceive so we didn't get valid data for this PhyID (inactive)
phyIDSetActive(currentPolledPhyID, false);
}
if (forcedPolledPhyID != -1) {
phyIDToPoll = forcedPolledPhyID;
forcedPolledPhyID = -1;
} else {
if (phyIDNoneActive()) {
nextPollType = PT_INACTIVE_ID;
} else if (phyIDAllActive()) {
nextPollType = PT_ACTIVE_ID;
}
switch (nextPollType) {
case PT_ACTIVE_ID: {
int8_t activePhyIDToPoll = phyIDNext(nextActivePhyID, true, false);
if (activePhyIDToPoll == -1) {
nextActivePhyID = 0;
nextPollType = PT_INACTIVE_ID;
} else {
phyIDToPoll = activePhyIDToPoll;
if (phyIDToPoll == SMARTPORT_PHYID_MAX) {
nextActivePhyID = 0;
if (!phyIDAllActive()) {
nextPollType = PT_INACTIVE_ID;
}
} else {
nextActivePhyID = phyIDToPoll + 1;
}
break;
}
FALLTHROUGH;
}
case PT_INACTIVE_ID: {
phyIDToPoll = phyIDNext(nextInactivePhyID, false, true);
nextInactivePhyID = (phyIDToPoll == SMARTPORT_PHYID_MAX ? 0 : phyIDToPoll + 1);
if (phyIDAnyActive()) {
nextPollType = PT_ACTIVE_ID;
}
break;
}
}
}
currentPolledPhyID = phyIDToPoll;
smartportMasterPhyIDFillCheckBits(&phyIDToPoll);
// poll
smartportMasterSendByte(SMARTPORT_FRAME_START);
smartportMasterSendByte(phyIDToPoll);
rxBufferLen = 0; // discard incomplete frames received during previous poll
}
static void smartportMasterForwardNextPayload(void)
{
smartportForwardData_t *request = forwardRequests + forwardRequestsStart;
/*forcedPolledPhyID = request->phyID; // force next poll to the request's phyID XXX disabled, doesn't seem necessary */
smartportMasterPhyIDFillCheckBits(&request->phyID);
smartportMasterSendByte(SMARTPORT_FRAME_START);
smartportMasterSendByte(request->phyID);
uint16_t checksum = 0;
uint8_t *payloadPtr = (uint8_t *)&request->payload;
for (uint8_t i = 0; i < sizeof(request->payload); ++i) {
uint8_t c = *payloadPtr++;
if ((c == SMARTPORT_FRAME_START) || (c == SMARTPORT_BYTESTUFFING_MARKER)) {
smartportMasterSendByte(SMARTPORT_BYTESTUFFING_MARKER);
smartportMasterSendByte(c ^ SMARTPORT_BYTESTUFFING_XOR_VALUE);
} else {
smartportMasterSendByte(c);
}
checksum += c;
}
checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
smartportMasterSendByte(checksum);
forwardRequestsStart = (forwardRequestsStart + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
}
static void decodeCellsData(uint32_t sdata)
{
uint8_t voltageStartIndex = sdata & 0xF;
uint8_t count = sdata >> 4 & 0xF;
uint16_t voltage1 = (sdata >> 8 & 0xFFF) * 2;
uint16_t voltage2 = (sdata >> 20 & 0xFFF) * 2;
if ((voltageStartIndex <= 4) && (count <= 6)) { // sanity check
cellsData_t *cd = &sensorsData.cells;
cd->count = count;
cd->voltage[voltageStartIndex] = voltage1;
cd->voltage[voltageStartIndex+1] = voltage2;
DEBUG_SET(DEBUG_SPM_CELLS, 0, cd->count);
for (uint8_t i = 0; i < 6; ++i) {
DEBUG_SET(DEBUG_SPM_CELLS, i+1, cd->voltage[i]);
}
}
}
static void decodeVS600Data(uint32_t sdata)
{
vs600Data_t *vs600 = &sensorsData.vs600;
vs600->power = (sdata >> 8) & 0xFF;
vs600->band = (sdata >> 16) & 0xFF;
vs600->channel = (sdata >> 24) & 0xFF;
DEBUG_SET(DEBUG_SPM_VS600, 0, sdata);
DEBUG_SET(DEBUG_SPM_VS600, 1, vs600->channel);
DEBUG_SET(DEBUG_SPM_VS600, 2, vs600->band);
DEBUG_SET(DEBUG_SPM_VS600, 3, vs600->power);
}
static void decodeAltitudeData(uint32_t sdata)
{
sensorsData.altitude = sdata * 5; // cm
DEBUG_SET(DEBUG_SPM_VARIO, 0, sensorsData.altitude);
}
static void decodeVarioData(uint32_t sdata)
{
sensorsData.vario = sdata * 2; // mm/s
DEBUG_SET(DEBUG_SPM_VARIO, 1, sensorsData.vario);
}
static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
{
switch (payload->valueId) {
case DATAID_CELLS:
decodeCellsData(payload->data);
sensorsData.cellsTimestamp = currentTimeUs;
break;
case DATAID_VS600:
decodeVS600Data(payload->data);
sensorsData.vs600Timestamp = currentTimeUs;
break;
case DATAID_ALTITUDE:
decodeAltitudeData(payload->data);
sensorsData.altitudeTimestamp = currentTimeUs;
break;
case DATAID_VARIO:
decodeVarioData(payload->data);
sensorsData.varioTimestamp = currentTimeUs;
break;
}
sensorPayloadCache[currentPolledPhyID] = *payload;
}
static void processPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
{
switch (payload->frameId) {
case PRIM_DISCARD_FRAME:
break;
case PRIM_DATA_FRAME:
processSensorPayload(payload, currentTimeUs);
break;
default:
if (forwardResponsesCount < SMARTPORT_FORWARD_REQUESTS_MAX) {
smartportForwardData_t *response = forwardResponses + forwardResponsesCount;
response->phyID = currentPolledPhyID;
response->payload = *payload;
forwardResponsesCount += 1;
}
break;
}
}
static void smartportMasterReceive(timeUs_t currentTimeUs)
{
static smartportFrameBuffer_u buffer;
static bool processByteStuffing = false;
if (!rxBufferLen) {
processByteStuffing = false;
}
while (serialRxBytesWaiting(smartportMasterSerialPort)) {
uint8_t c = serialRead(smartportMasterSerialPort);
if (currentPolledPhyID == -1) { // We did not poll a sensor or a packet has already been received and processed
continue;
}
if (processByteStuffing) {
c ^= SMARTPORT_BYTESTUFFING_XOR_VALUE;
processByteStuffing = false;
} else if (c == SMARTPORT_BYTESTUFFING_MARKER) {
processByteStuffing = true;
continue;
}
buffer.bytes[rxBufferLen] = c;
rxBufferLen += 1;
if (rxBufferLen == sizeof(buffer)) { // frame complete
uint8_t calcCRC = frskyCheckSum((uint8_t *)&buffer.frame.payload, sizeof(buffer.frame.payload));
if (buffer.frame.crc == calcCRC) {
phyIDSetActive(currentPolledPhyID, true);
processPayload(&buffer.frame.payload, currentTimeUs);
}
currentPolledPhyID = -1; // previously polled PhyID has answered, not expecting more data until next poll
rxBufferLen = 0; // reset buffer
}
}
}
bool smartportMasterGetSensorPayload(uint8_t phyID, smartPortPayload_t *payload)
{
if ((phyID > SMARTPORT_PHYID_MAX) || !phyIDIsActive(phyID)) {
return false;
}
*payload = sensorPayloadCache[phyID];
return true;
}
bool smartportMasterHasForwardResponse(uint8_t phyID)
{
for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
smartportForwardData_t *response = forwardResponses + i;
if (response->phyID == phyID) {
return true;
}
}
return false;
}
bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *payload)
{
for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
smartportForwardData_t *response = forwardResponses + i;
if (response->phyID == phyID) {
*payload = response->payload;
forwardResponsesCount -= 1;
memmove(response, response + 1, (forwardResponsesCount - i) * sizeof(*response));
return true;
}
}
return false;
}
static uint8_t forwardRequestCount(void)
{
if (forwardRequestsStart > forwardRequestsEnd) {
return SMARTPORT_FORWARD_REQUESTS_MAX - forwardRequestsStart + forwardRequestsEnd;
} else {
return forwardRequestsEnd - forwardRequestsStart;
}
}
bool smartportMasterForward(uint8_t phyID, smartPortPayload_t *payload)
{
if (forwardRequestCount() == SMARTPORT_FORWARD_REQUESTS_MAX) {
return false;
}
smartportForwardData_t *request = forwardRequests + forwardRequestsEnd;
request->phyID = phyID;
request->payload = *payload;
forwardRequestsEnd = (forwardRequestsEnd + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
return true;
}
void smartportMasterHandle(timeUs_t currentTimeUs)
{
static timeUs_t pollTimestamp = 0;
if (!smartportMasterSerialPort) {
return;
}
if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) {
if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
smartportMasterForwardNextPayload();
} else {
smartportMasterPoll();
}
pollTimestamp = currentTimeUs;
} else {
smartportMasterReceive(currentTimeUs);
}
}
cellsData_t *smartportMasterGetCellsData(void)
{
if (micros() - sensorsData.cellsTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
return NULL;
}
return &sensorsData.cells;
}
vs600Data_t *smartportMasterGetVS600Data(void)
{
if (micros() - sensorsData.vs600Timestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
return NULL;
}
return &sensorsData.vs600;
}
bool smartportMasterPhyIDIsActive(uint8_t phyID)
{
return phyIDIsActive(phyID);
}
int8_t smartportMasterStripPhyIDCheckBits(uint8_t phyID)
{
uint8_t smartportPhyID = phyID & 0x1F;
uint8_t phyIDCheck = smartportPhyID;
smartportMasterPhyIDFillCheckBits(&phyIDCheck);
return phyID == phyIDCheck ? smartportPhyID : -1;
}
#endif

87
src/main/io/smartport_master.h Normal file
View file

@ -0,0 +1,87 @@
/*
* This file is part of iNav
*
* iNav free software. You can redistribute
* this software and/or modify this software 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.
*
* iNav 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <common/time.h>
#include "config/parameter_group.h"
#include <telemetry/smartport.h>
#if defined(USE_SMARTPORT_MASTER)
typedef struct {
bool halfDuplex;
bool inverted;
} smartportMasterConfig_t;
PG_DECLARE(smartportMasterConfig_t, smartportMasterConfig);
typedef struct {
int8_t count;
int16_t voltage[6];
} cellsData_t;
typedef enum {
VS600_BAND_A,
VS600_BAND_B,
VS600_BAND_C,
VS600_BAND_D,
VS600_BAND_E,
VS600_BAND_F,
} vs600Band_e;
typedef enum {
VS600_POWER_PIT,
VS600_POWER_25MW,
VS600_POWER_200MW,
VS600_POWER_600MW,
} vs600Power_e;
typedef struct {
vs600Band_e band;
uint8_t channel;
vs600Power_e power;
} vs600Data_t;
bool smartportMasterInit(void);
void smartportMasterHandle(timeUs_t currentTimeUs);
bool smartportMasterPhyIDIsActive(uint8_t phyID);
int8_t smartportMasterStripPhyIDCheckBits(uint8_t phyID);
// Returns latest received SmartPort payload for phyID
bool smartportMasterGetSensorPayload(uint8_t phyID, smartPortPayload_t *payload);
// Forwarding
bool smartportMasterForward(uint8_t phyID, smartPortPayload_t *payload);
bool smartportMasterHasForwardResponse(uint8_t phyID);
bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *payload);
// Returns latest Cells data or NULL if the data is too old
cellsData_t *smartportMasterGetCellsData(void);
vs600Data_t *smartportMasterGetVS600Data(void);
#endif /* USE_SMARTPORT_MASTER */

6
src/main/programming/global_functions.c
View file

@ -135,6 +135,12 @@ void globalFunctionsProcess(int8_t functionId) {
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE);
}
break;
case GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT:
if(conditionValue){
globalFunctionValues[GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT] = globalFunctionsStates[functionId].value;
GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT);
}
break;
}
}
}

2
src/main/programming/global_functions.h
View file

@ -39,6 +39,7 @@ typedef enum {
GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE, // 7
GLOBAL_FUNCTION_ACTION_SET_VTX_BAND, // 8
GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL, // 9
GLOBAL_FUNCTION_ACTION_SET_OSD_LAYOUT, // 10
GLOBAL_FUNCTION_ACTION_LAST
} globalFunctionActions_e;
@ -50,6 +51,7 @@ typedef enum {
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE = (1 << 6),
GLOBAL_FUNCTION_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
} globalFunctionFlags_t;
typedef struct globalFunction_s {

3
src/main/scheduler/scheduler.h
View file

@ -116,6 +116,9 @@ typedef enum {
#ifdef USE_RPM_FILTER
TASK_RPM_FILTER,
#endif
#if defined(USE_SMARTPORT_MASTER)
TASK_SMARTPORT_MASTER,
#endif
#ifdef USE_IRLOCK
TASK_IRLOCK,
#endif

100
src/main/telemetry/mavlink.c
View file

@ -82,6 +82,60 @@
#define TELEMETRY_MAVLINK_MAXRATE 50
#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE)
/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */
typedef enum APM_PLANE_MODE
{
PLANE_MODE_MANUAL=0,
PLANE_MODE_CIRCLE=1,
PLANE_MODE_STABILIZE=2,
PLANE_MODE_TRAINING=3,
PLANE_MODE_ACRO=4,
PLANE_MODE_FLY_BY_WIRE_A=5,
PLANE_MODE_FLY_BY_WIRE_B=6,
PLANE_MODE_CRUISE=7,
PLANE_MODE_AUTOTUNE=8,
PLANE_MODE_AUTO=10,
PLANE_MODE_RTL=11,
PLANE_MODE_LOITER=12,
PLANE_MODE_TAKEOFF=13,
PLANE_MODE_AVOID_ADSB=14,
PLANE_MODE_GUIDED=15,
PLANE_MODE_INITIALIZING=16,
PLANE_MODE_QSTABILIZE=17,
PLANE_MODE_QHOVER=18,
PLANE_MODE_QLOITER=19,
PLANE_MODE_QLAND=20,
PLANE_MODE_QRTL=21,
PLANE_MODE_QAUTOTUNE=22,
PLANE_MODE_ENUM_END=23,
} APM_PLANE_MODE;
/** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */
typedef enum APM_COPTER_MODE
{
COPTER_MODE_STABILIZE=0,
COPTER_MODE_ACRO=1,
COPTER_MODE_ALT_HOLD=2,
COPTER_MODE_AUTO=3,
COPTER_MODE_GUIDED=4,
COPTER_MODE_LOITER=5,
COPTER_MODE_RTL=6,
COPTER_MODE_CIRCLE=7,
COPTER_MODE_LAND=9,
COPTER_MODE_DRIFT=11,
COPTER_MODE_SPORT=13,
COPTER_MODE_FLIP=14,
COPTER_MODE_AUTOTUNE=15,
COPTER_MODE_POSHOLD=16,
COPTER_MODE_BRAKE=17,
COPTER_MODE_THROW=18,
COPTER_MODE_AVOID_ADSB=19,
COPTER_MODE_GUIDED_NOGPS=20,
COPTER_MODE_SMART_RTL=21,
COPTER_MODE_ENUM_END=22,
} APM_COPTER_MODE;
static serialPort_t *mavlinkPort = NULL;
static serialPortConfig_t *portConfig;
@ -108,9 +162,45 @@ static mavlink_status_t mavRecvStatus;
static uint8_t mavSystemId = 1;
static uint8_t mavComponentId = MAV_COMP_ID_SYSTEM_CONTROL;
// MANUAL, ACRO, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, LAUNCH, FAILSAFE
static uint8_t inavToArduCopterMap[FLM_COUNT] = { 1, 1, 0, 0, 2, 16, 6, 3, 18, 0 };
static uint8_t inavToArduPlaneMap[FLM_COUNT] = { 0, 4, 2, 2, 5, 1, 11, 10, 15, 2 };
APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
{
switch (flightMode)
{
case FLM_ACRO: return COPTER_MODE_ACRO;
case FLM_ACRO_AIR: return COPTER_MODE_ACRO;
case FLM_ANGLE: return COPTER_MODE_STABILIZE;
case FLM_HORIZON: return COPTER_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
case FLM_RTH: return COPTER_MODE_RTL;
case FLM_MISSION: return COPTER_MODE_AUTO;
case FLM_LAUNCH: return COPTER_MODE_THROW;
case FLM_FAILSAFE: return COPTER_MODE_RTL;
default: return COPTER_MODE_ENUM_END;
}
}
APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
{
switch (flightMode)
{
case FLM_MANUAL: return PLANE_MODE_MANUAL;
case FLM_ACRO: return PLANE_MODE_ACRO;
case FLM_ACRO_AIR: return PLANE_MODE_ACRO;
case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A;
case FLM_HORIZON: return PLANE_MODE_STABILIZE;
case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
case FLM_RTH: return PLANE_MODE_RTL;
case FLM_MISSION: return PLANE_MODE_AUTO;
case FLM_CRUISE: return PLANE_MODE_CRUISE;
case FLM_LAUNCH: return PLANE_MODE_TAKEOFF;
case FLM_FAILSAFE: return PLANE_MODE_RTL;
default: return PLANE_MODE_ENUM_END;
}
}
static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum)
{
@ -468,10 +558,10 @@ void mavlinkSendHUDAndHeartbeat(void)
uint8_t mavCustomMode;
if (STATE(FIXED_WING_LEGACY)) {
mavCustomMode = inavToArduPlaneMap[flm];
mavCustomMode = (uint8_t)inavToArduPlaneMap(flm);
}
else {
mavCustomMode = inavToArduCopterMap[flm];
mavCustomMode = (uint8_t)inavToArduCopterMap(flm);
}
if (flm != FLM_MANUAL) {