1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 09:16:07 +03:00

Merge remote-tracking branch 'betaflight/master' into RP2350

This commit is contained in:
blckmn 2024-12-31 19:14:01 +11:00
commit 04a7cc0ea1
79 changed files with 1905 additions and 300 deletions

View file

@ -16,7 +16,6 @@
#
# The target to build, see BASE_TARGETS below
DEFAULT_TARGET ?= STM32F405
TARGET ?=
CONFIG ?=
@ -124,12 +123,6 @@ FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
# import config handling
include $(MAKE_SCRIPT_DIR)/config.mk
ifeq ($(CONFIG),)
ifeq ($(TARGET),)
TARGET := $(DEFAULT_TARGET)
SKIPCHECKS := yes
endif
endif
# default xtal value
HSE_VALUE ?= 8000000
@ -141,7 +134,9 @@ TARGET_PLATFORM := $(notdir $(patsubst %/,%,$(subst target/$(TARGET)/,, $(di
TARGET_PLATFORM_DIR := $(PLATFORM_DIR)/$(TARGET_PLATFORM)
LINKER_DIR := $(TARGET_PLATFORM_DIR)/link
ifneq ($(TARGET),)
include $(TARGET_PLATFORM_DIR)/target/$(TARGET)/target.mk
endif
REVISION := norevision
ifeq ($(shell git diff --shortstat),)
@ -182,6 +177,8 @@ endif
VPATH := $(VPATH):$(MAKE_SCRIPT_DIR)
ifneq ($(TARGET),)
# start specific includes
ifeq ($(TARGET_MCU),)
$(error No TARGET_MCU specified. Is the target.mk valid for $(TARGET)?)
@ -202,9 +199,6 @@ SIZE_OPTIMISED_SRC :=
include $(TARGET_PLATFORM_DIR)/mk/$(TARGET_MCU_FAMILY).mk
# openocd specific includes
include $(MAKE_SCRIPT_DIR)/openocd.mk
# Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
ifeq ($(TARGET_FLASH_SIZE),)
ifneq ($(MCU_FLASH_SIZE),)
@ -221,8 +215,12 @@ DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE)
endif
TARGET_DIR = $(TARGET_PLATFORM_DIR)/target/$(TARGET)
endif # TARGET specified
.DEFAULT_GOAL := hex
# openocd specific includes
include $(MAKE_SCRIPT_DIR)/openocd.mk
.DEFAULT_GOAL := all
INCLUDE_DIRS := $(INCLUDE_DIRS) \
$(ROOT)/lib/main/MAVLink
@ -234,7 +232,7 @@ VPATH := $(VPATH):$(TARGET_DIR)
include $(MAKE_SCRIPT_DIR)/source.mk
ifneq ($(SKIPCHECKS),yes)
ifneq ($(TARGET),)
ifneq ($(filter-out $(SRC),$(SPEED_OPTIMISED_SRC)),)
$(error Speed optimised sources not valid: $(strip $(filter-out $(SRC),$(SPEED_OPTIMISED_SRC))))
endif
@ -642,7 +640,7 @@ help: Makefile mk/tools.mk
@echo "Or:"
@echo " make <config-target> [V=<verbosity>] [OPTIONS=\"<options>\"] [EXTRA_FLAGS=\"<extra_flags>\"]"
@echo ""
@echo "To pupulate configuration targets:"
@echo "To populate configuration targets:"
@echo " make configs"
@echo ""
@echo "Valid TARGET values are: $(BASE_TARGETS)"

View file

@ -1,7 +1,9 @@
PG_SRC = \
pg/adc.c \
pg/alt_hold.c \
pg/autopilot.c \
pg/alt_hold_multirotor.c \
pg/alt_hold_wing.c \
pg/autopilot_multirotor.c \
pg/autopilot_wing.c \
pg/beeper.c \
pg/beeper_dev.c \
pg/board.c \
@ -15,17 +17,20 @@ PG_SRC = \
pg/gimbal.c \
pg/gps.c \
pg/gps_lap_timer.c \
pg/gps_rescue.c \
pg/gps_rescue_multirotor.c \
pg/gps_rescue_wing.c \
pg/gyrodev.c \
pg/max7456.c \
pg/mco.c \
pg/motor.c \
pg/msp.c \
pg/pg.c \
pg/pilot.c \
pg/piniobox.c \
pg/pinio.c \
pg/pin_pull_up_down.c \
pg/pos_hold.c \
pg/pos_hold_multirotor.c \
pg/pos_hold_wing.c \
pg/rcdevice.c \
pg/rpm_filter.c \
pg/rx.c \
@ -153,11 +158,14 @@ COMMON_SRC = \
fc/rc_adjustments.c \
fc/rc_controls.c \
fc/rc_modes.c \
flight/alt_hold.c \
flight/autopilot.c \
flight/alt_hold_multirotor.c \
flight/alt_hold_wing.c \
flight/autopilot_multirotor.c \
flight/autopilot_wing.c \
flight/dyn_notch_filter.c \
flight/failsafe.c \
flight/gps_rescue.c \
flight/gps_rescue_multirotor.c \
flight/gps_rescue_wing.c \
flight/imu.c \
flight/mixer.c \
flight/mixer_init.c \
@ -165,7 +173,8 @@ COMMON_SRC = \
flight/pid.c \
flight/pid_init.c \
flight/position.c \
flight/pos_hold.c \
flight/pos_hold_multirotor.c \
flight/pos_hold_wing.c \
flight/rpm_filter.c \
flight/servos.c \
flight/servos_tricopter.c \
@ -209,7 +218,8 @@ COMMON_SRC = \
cms/cms_menu_blackbox.c \
cms/cms_menu_failsafe.c \
cms/cms_menu_firmware.c \
cms/cms_menu_gps_rescue.c \
cms/cms_menu_gps_rescue_multirotor.c \
cms/cms_menu_gps_rescue_wing.c \
cms/cms_menu_gps_lap_timer.c \
cms/cms_menu_imu.c \
cms/cms_menu_ledstrip.c \
@ -512,7 +522,8 @@ SIZE_OPTIMISED_SRC += \
cms/cms_menu_blackbox.c \
cms/cms_menu_failsafe.c \
cms/cms_menu_firmware.c \
cms/cms_menu_gps_rescue.c \
cms/cms_menu_gps_rescue_multirotor.c \
cms/cms_menu_gps_rescue_wing.c \
cms/cms_menu_gps_lap_timer.c \
cms/cms_menu_imu.c \
cms/cms_menu_ledstrip.c \

View file

@ -81,8 +81,9 @@
#include "pg/alt_hold.h"
#include "pg/autopilot.h"
#include "pg/motor.h"
#include "pg/rx.h"
#include "pg/pilot.h"
#include "pg/pos_hold.h"
#include "pg/rx.h"
#include "rx/rx.h"
@ -744,7 +745,7 @@ static void writeIntraframe(void)
if (testBlackboxCondition(CONDITION(ACC))) {
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(CONDITION(ATTITUDE))) {
blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT);
}
@ -929,7 +930,7 @@ static void writeInterframe(void)
if (testBlackboxCondition(CONDITION(ACC))) {
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
}
if (testBlackboxCondition(CONDITION(ATTITUDE))) {
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT);
}
@ -1243,14 +1244,14 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_F[i] = lrintf(pidData[i].F);
#ifdef USE_WING
blackboxCurrent->axisPID_S[i] = lrintf(pidData[i].S);
#endif
#endif
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale);
blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale);
#if defined(USE_ACC)
blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]);
STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order");
blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w
blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF); //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w
#endif
#ifdef USE_MAG
blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);
@ -1694,6 +1695,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
#ifndef USE_WING
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d", apConfig()->landing_altitude_m);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d", apConfig()->hover_throttle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d", apConfig()->throttle_min);
@ -1708,6 +1710,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d", apConfig()->position_A);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d", apConfig()->position_cutoff);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d", apConfig()->max_angle);
#endif // !USE_WING
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
@ -1784,6 +1787,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
#ifdef USE_GPS_RESCUE
#ifndef USE_WING
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM)
@ -1808,21 +1812,25 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP)
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag)
#endif // USE_MAG
#endif // !USE_WING
#endif // USE_GPS_RESCUE
#endif // USE_GPS
#ifdef USE_ALTITUDE_HOLD
#ifndef USE_WING
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND, "%d", altHoldConfig()->alt_hold_deadband);
#endif
#endif // !USE_WING
#endif // USE_ALTITUDE_HOLD
#ifdef USE_POSITION_HOLD
#ifndef USE_WING
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND, "%d", posHoldConfig()->pos_hold_deadband);
#endif // !USE_WING
#endif
#ifdef USE_WING

View file

@ -139,6 +139,7 @@ bool cliMode = false;
#include "pg/max7456.h"
#include "pg/mco.h"
#include "pg/motor.h"
#include "pg/pilot.h"
#include "pg/pinio.h"
#include "pg/pin_pull_up_down.h"
#include "pg/pg.h"
@ -209,6 +210,7 @@ static bool signatureUpdated = false;
#endif // USE_BOARD_INFO
static const char* const emptyName = "-";
static const char* const invalidName = "INVALID";
#define MAX_CHANGESET_ID_LENGTH 8
#define MAX_DATE_LENGTH 20
@ -1274,7 +1276,7 @@ static void cliAux(const char *cmdName, char *cmdline)
static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig, const serialConfig_t *serialConfigDefault, const char *headingStr)
{
const char *format = "serial %d %d %ld %ld %ld %ld";
const char *format = "serial %s %d %ld %ld %ld %ld";
headingStr = cliPrintSectionHeading(dumpMask, false, headingStr);
for (unsigned i = 0; i < ARRAYLEN(serialConfig->portConfigs); i++) {
if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) {
@ -1285,7 +1287,7 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig
equalsDefault = !memcmp(&serialConfig->portConfigs[i], &serialConfigDefault->portConfigs[i], sizeof(serialConfig->portConfigs[i]));
headingStr = cliPrintSectionHeading(dumpMask, !equalsDefault, headingStr);
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
serialConfigDefault->portConfigs[i].identifier,
serialName(serialConfigDefault->portConfigs[i].identifier, invalidName),
serialConfigDefault->portConfigs[i].functionMask,
baudRates[serialConfigDefault->portConfigs[i].msp_baudrateIndex],
baudRates[serialConfigDefault->portConfigs[i].gps_baudrateIndex],
@ -1294,7 +1296,7 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig
);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
serialConfig->portConfigs[i].identifier,
serialName(serialConfig->portConfigs[i].identifier, invalidName),
serialConfig->portConfigs[i].functionMask,
baudRates[serialConfig->portConfigs[i].msp_baudrateIndex],
baudRates[serialConfig->portConfigs[i].gps_baudrateIndex],
@ -1306,40 +1308,58 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig
static void cliSerial(const char *cmdName, char *cmdline)
{
const char *format = "serial %d %d %ld %ld %ld %ld";
const char *format = "serial %s %d %ld %ld %ld %ld";
if (isEmpty(cmdline)) {
printSerial(DUMP_MASTER, serialConfig(), NULL, NULL);
return;
}
serialPortConfig_t portConfig;
memset(&portConfig, 0 , sizeof(portConfig));
uint8_t validArgumentCount = 0;
const char *ptr = cmdline;
int val = atoi(ptr++);
serialPortConfig_t *currentConfig = serialFindPortConfigurationMutable(val);
if (currentConfig) {
portConfig.identifier = val;
validArgumentCount++;
char *ptr = cmdline;
char *tok = strsep(&ptr, " ");
serialPortIdentifier_e identifier = findSerialPortByName(tok, strcasecmp);
if (identifier == SERIAL_PORT_NONE) {
char *eptr;
identifier = strtoul(tok, &eptr, 10);
if (*eptr) {
// parsing ended before end of token indicating an invalid identifier
identifier = SERIAL_PORT_NONE;
} else {
// correction for legacy configuration where UART1 == 0
if (identifier >= SERIAL_PORT_LEGACY_START_IDENTIFIER && identifier < SERIAL_PORT_START_IDENTIFIER) {
identifier += SERIAL_PORT_UART1;
}
}
}
ptr = nextArg(ptr);
if (ptr) {
val = strtoul(ptr, NULL, 10);
serialPortConfig_t *currentConfig = serialFindPortConfigurationMutable(identifier);
if (!currentConfig) {
cliShowParseError(cmdName);
return;
}
portConfig.identifier = identifier;
validArgumentCount++;
tok = strsep(&ptr, " ");
if (tok) {
int val = strtoul(tok, NULL, 10);
portConfig.functionMask = val;
validArgumentCount++;
}
for (int i = 0; i < 4; i ++) {
ptr = nextArg(ptr);
if (!ptr) {
tok = strsep(&ptr, " ");
if (!tok) {
break;
}
val = atoi(ptr);
int val = atoi(tok);
uint8_t baudRateIndex = lookupBaudRateIndex(val);
if (baudRates[baudRateIndex] != (uint32_t) val) {
@ -1384,14 +1404,13 @@ static void cliSerial(const char *cmdName, char *cmdline)
memcpy(currentConfig, &portConfig, sizeof(portConfig));
cliDumpPrintLinef(0, false, format,
portConfig.identifier,
serialName(portConfig.identifier, invalidName),
portConfig.functionMask,
baudRates[portConfig.msp_baudrateIndex],
baudRates[portConfig.gps_baudrateIndex],
baudRates[portConfig.telemetry_baudrateIndex],
baudRates[portConfig.blackbox_baudrateIndex]
);
}
#if defined(USE_SERIAL_PASSTHROUGH)

View file

@ -95,6 +95,7 @@
#include "pg/msp.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/pilot.h"
#include "pg/pinio.h"
#include "pg/piniobox.h"
#include "pg/pos_hold.h"
@ -1073,6 +1074,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_GPS_NMEA_CUSTOM_COMMANDS, VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, NMEA_CUSTOM_COMMANDS_MAX_LENGTH, STRING_FLAGS_NONE }, PG_GPS_CONFIG, offsetof(gpsConfig_t, nmeaCustomCommands) },
#ifdef USE_GPS_RESCUE
#ifndef USE_WING
// PG_GPS_RESCUE
{ PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minStartDistM) },
{ PARAM_NAME_GPS_RESCUE_ALT_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) },
@ -1102,6 +1104,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_MAG
{ PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif // USE_MAG
#endif // !USE_WING
#endif // USE_GPS_RESCUE
#ifdef USE_GPS_LAP_TIMER
@ -1118,14 +1121,18 @@ const clivalue_t valueTable[] = {
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
#ifdef USE_ALTITUDE_HOLD
#ifndef USE_WING
{ PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
{ PARAM_NAME_ALT_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 70 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_deadband) },
#endif
#endif // !USE_WING
#endif // USE_ALTITUDE_HOLD
#ifdef USE_POSITION_HOLD
#ifndef USE_WING
{ PARAM_NAME_POS_HOLD_WITHOUT_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_without_mag) },
{ PARAM_NAME_POS_HOLD_DEADBAND, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_deadband) },
#endif
#endif // !USE_WING
#endif // USE_POSITION_HOLD
// PG_PID_CONFIG
{ PARAM_NAME_PID_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) },
@ -1866,6 +1873,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_ALTITUDE_D_LPF, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
// PG_AUTOPILOT
#ifndef USE_WING
{ PARAM_NAME_LANDING_ALTITUDE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, landing_altitude_m) },
{ PARAM_NAME_HOVER_THROTTLE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1100, 1700 }, PG_AUTOPILOT, offsetof(apConfig_t, hover_throttle) },
{ PARAM_NAME_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1050, 1400 }, PG_AUTOPILOT, offsetof(apConfig_t, throttle_min) },
@ -1880,6 +1888,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_POSITION_A, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_AUTOPILOT, offsetof(apConfig_t, position_A) },
{ PARAM_NAME_POSITION_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_AUTOPILOT, offsetof(apConfig_t, position_cutoff) },
{ PARAM_NAME_AP_MAX_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 }, PG_AUTOPILOT, offsetof(apConfig_t, max_angle) },
#endif // !USE_WING
// PG_MODE_ACTIVATION_CONFIG
#if defined(USE_CUSTOM_BOX_NAMES)

View file

@ -25,6 +25,8 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_CMS_GPS_RESCUE_MENU
#include "cli/settings.h"
@ -242,3 +244,5 @@ CMS_Menu cmsx_menuGpsRescue = {
};
#endif
#endif // !USE_WING

View file

@ -0,0 +1,114 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include "platform.h"
#ifdef USE_WING
#ifdef USE_CMS_GPS_RESCUE_MENU
#include "cli/settings.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_gps_rescue.h"
#include "config/feature.h"
#include "config/config.h"
#include "flight/position.h"
#include "pg/autopilot.h"
#include "pg/gps_rescue.h"
static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);
return NULL;
}
static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_Entry *self)
{
UNUSED(pDisp);
UNUSED(self);
return NULL;
}
const OSD_Entry cms_menuGpsRescuePidEntries[] =
{
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
{"BACK", OME_Back, NULL, NULL},
{NULL, OME_END, NULL, NULL}
};
CMS_Menu cms_menuGpsRescuePid = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUGPSRPID",
.GUARD_type = OME_MENU,
#endif
.onEnter = cms_menuGpsRescuePidOnEnter,
.onExit = cms_menuGpsRescuePidOnExit,
.onDisplayUpdate = NULL,
.entries = cms_menuGpsRescuePidEntries,
};
static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);
return NULL;
}
static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self)
{
UNUSED(pDisp);
UNUSED(self);
return NULL;
}
const OSD_Entry cmsx_menuGpsRescueEntries[] =
{
{"--- GPS RESCUE ---", OME_Label, NULL, NULL},
{"BACK", OME_Back, NULL, NULL},
{NULL, OME_END, NULL, NULL}
};
CMS_Menu cmsx_menuGpsRescue = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUGPSRES",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_menuGpsRescueOnEnter,
.onExit = cmsx_menuGpsRescueOnExit,
.onDisplayUpdate = NULL,
.entries = cmsx_menuGpsRescueEntries,
};
#endif
#endif // USE_WING

View file

@ -106,13 +106,6 @@ pidProfile_t *currentPidProfile;
#define RX_SPI_DEFAULT_PROTOCOL 0
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 2);
PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
.craftName = { 0 },
.pilotName = { 0 },
);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3);
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,

View file

@ -25,20 +25,11 @@
#include "pg/pg.h"
#define MAX_NAME_LENGTH 16u
typedef enum {
CONFIGURATION_STATE_UNCONFIGURED = 0,
CONFIGURATION_STATE_CONFIGURED,
} configurationState_e;
typedef struct pilotConfig_s {
char craftName[MAX_NAME_LENGTH + 1];
char pilotName[MAX_NAME_LENGTH + 1];
} pilotConfig_t;
PG_DECLARE(pilotConfig_t, pilotConfig);
typedef struct systemConfig_s {
uint8_t pidProfileIndex;
uint8_t activeRateProfile;

View file

@ -40,6 +40,9 @@ typedef struct serialDefaultPin_s {
} serialDefaultPin_t;
static const serialDefaultPin_t serialDefaultPin[] = {
#ifdef USE_UART0
{ SERIAL_PORT_UART0, IO_TAG(UART0_RX_PIN), IO_TAG(UART0_TX_PIN), IO_TAG(INVERTER_PIN_UART0) },
#endif
#ifdef USE_UART1
{ SERIAL_PORT_USART1, IO_TAG(UART1_RX_PIN), IO_TAG(UART1_TX_PIN), IO_TAG(INVERTER_PIN_UART1) },
#endif

View file

@ -44,15 +44,18 @@ static const struct serialPortVTable tcpVTable; // Forward
static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
static bool tcpPortInitialized[SERIAL_PORT_COUNT];
static bool tcpStart = false;
bool tcpIsStart(void)
{
return tcpStart;
}
static void onData(dyad_Event *e)
{
tcpPort_t* s = (tcpPort_t*)(e->udata);
tcpDataIn(s, (uint8_t*)e->data, e->size);
}
static void onClose(dyad_Event *e)
{
tcpPort_t* s = (tcpPort_t*)(e->udata);
@ -63,6 +66,7 @@ static void onClose(dyad_Event *e)
s->connected = false;
}
}
static void onAccept(dyad_Event *e)
{
tcpPort_t* s = (tcpPort_t*)(e->udata);
@ -81,6 +85,7 @@ static void onAccept(dyad_Event *e)
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
}
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
{
if (tcpPortInitialized[id]) {
@ -93,6 +98,7 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
// TODO: clean up & re-init
return NULL;
}
if (pthread_mutex_init(&s->rxLock, NULL) != 0) {
fprintf(stderr, "RX mutex init failed - %d\n", errno);
// TODO: clean up & re-init
@ -118,17 +124,19 @@ static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
return s;
}
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
serialPort_t *serTcpOpen(serialPortIdentifier_e identifier, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
{
tcpPort_t *s = NULL;
#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
if (id >= 0 && id < SERIAL_PORT_COUNT) {
s = tcpReconfigure(&tcpSerialPorts[id], id);
int id = findSerialPortIndexByIdentifier(identifier);
if (id >= 0 && id < (int)ARRAYLEN(tcpSerialPorts)) {
s = tcpReconfigure(&tcpSerialPorts[id], id);
}
#endif
if (!s)
if (!s) {
return NULL;
}
s->port.vTable = &tcpVTable;

View file

@ -23,6 +23,7 @@
#include <netinet/in.h>
#include <pthread.h>
#include "dyad.h"
#include "io/serial.h"
#define RX_BUFFER_SIZE 1400
#define TX_BUFFER_SIZE 1400
@ -41,7 +42,7 @@ typedef struct {
uint8_t id;
} tcpPort_t;
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options);
serialPort_t *serTcpOpen(serialPortIdentifier_e id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options);
// tcpPort API
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size);

View file

@ -71,6 +71,10 @@
UART_BUFFER(UART_RX_BUFFER_ATTRIBUTE, n, R); struct dummy_s \
/**/
#ifdef USE_UART0
UART_BUFFERS(0);
#endif
#ifdef USE_UART1
UART_BUFFERS(1);
#endif
@ -136,6 +140,9 @@ uartDeviceIdx_e uartDeviceIdxFromIdentifier(serialPortIdentifier_e identifier)
// table is for UART only to save space (LPUART is handled separately)
#define _R(id, dev) [id] = (dev) + 1
static const uartDeviceIdx_e uartMap[] = {
#ifdef USE_UART0
_R(SERIAL_PORT_UART0, UARTDEV_0),
#endif
#ifdef USE_UART1
_R(SERIAL_PORT_USART1, UARTDEV_1),
#endif

View file

@ -132,6 +132,9 @@
// compressed index of UART/LPUART. Direct index into uartDevice[]
typedef enum {
UARTDEV_INVALID = -1,
#ifdef USE_UART0
UARTDEV_0,
#endif
#ifdef USE_UART1
UARTDEV_1,
#endif
@ -285,6 +288,10 @@ void uartTxMonitor(uartPort_t *s);
UART_BUFFER(extern, n, T); struct dummy_s \
/**/
#ifdef USE_UART0
UART_BUFFERS_EXTERN(0);
#endif
#ifdef USE_UART1
UART_BUFFERS_EXTERN(1);
#endif

View file

@ -226,6 +226,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_LED_PROFILE,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .switchPositions = 3 }
}, {
.adjustmentFunction = ADJUSTMENT_LED_DIMMER,
.mode = ADJUSTMENT_MODE_SELECT,
.data = { .switchPositions = 100 }
}
};
@ -264,6 +268,8 @@ static const char * const adjustmentLabels[] = {
"ROLL F",
"YAW F",
"OSD PROFILE",
"LED PROFILE",
"LED DIMMER",
};
static int adjustmentRangeNameIndex = 0;
@ -641,6 +647,13 @@ static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, ui
if (getLedProfile() != position) {
setLedProfile(position);
}
#endif
break;
case ADJUSTMENT_LED_DIMMER:
#ifdef USE_LED_STRIP
if (getLedBrightness() != position) {
setLedBrightness(position);
}
#endif
break;

View file

@ -62,6 +62,7 @@ typedef enum {
ADJUSTMENT_YAW_F,
ADJUSTMENT_OSD_PROFILE,
ADJUSTMENT_LED_PROFILE,
ADJUSTMENT_LED_DIMMER,
ADJUSTMENT_FUNCTION_COUNT
} adjustmentFunction_e;

View file

@ -17,15 +17,5 @@
#pragma once
#include "pg/alt_hold.h"
#ifdef USE_ALTITUDE_HOLD
#include "common/time.h"
#define ALTHOLD_TASK_RATE_HZ 100 // hz
void altHoldInit(void);
void updateAltHold(timeUs_t currentTimeUs);
bool isAltHoldActive(void);
#endif
#include "flight/alt_hold_multirotor.h"
#include "flight/alt_hold_wing.h"

View file

@ -16,6 +16,9 @@
*/
#include "platform.h"
#ifndef USE_WING
#include "math.h"
#ifdef USE_ALTITUDE_HOLD
@ -90,8 +93,8 @@ void altHoldProcessTransitions(void) {
void altHoldUpdateTargetAltitude(void)
{
// User can adjust the target altitude with throttle, but only when
// - throttle is outside deadband, and
// - throttle is not low (zero), and
// - throttle is outside deadband, and
// - throttle is not low (zero), and
// - deadband is not configured to zero
float stickFactor = 0.0f;
@ -154,3 +157,5 @@ bool isAltHoldActive(void) {
return altHold.isActive;
}
#endif
#endif // !USE_WING

View file

@ -0,0 +1,35 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include "pg/alt_hold.h"
#ifdef USE_ALTITUDE_HOLD
#include "common/time.h"
#define ALTHOLD_TASK_RATE_HZ 100 // hz
void altHoldInit(void);
void updateAltHold(timeUs_t currentTimeUs);
bool isAltHoldActive(void);
#endif
#endif // !USE_WING

View file

@ -0,0 +1,59 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#include "math.h"
#ifdef USE_ALTITUDE_HOLD
#include "build/debug.h"
#include "common/maths.h"
#include "config/config.h"
#include "fc/rc.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/position.h"
#include "rx/rx.h"
#include "pg/autopilot.h"
#include "alt_hold.h"
void altHoldReset(void)
{
}
void altHoldInit(void)
{
}
void updateAltHold(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
}
bool isAltHoldActive(void) {
return false;
}
#endif // USE_ALTITUDE_HOLD
#endif // USE_WING

View file

@ -0,0 +1,35 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include "pg/alt_hold.h"
#ifdef USE_ALTITUDE_HOLD
#include "common/time.h"
#define ALTHOLD_TASK_RATE_HZ 100 // hz
void altHoldInit(void);
void updateAltHold(timeUs_t currentTimeUs);
bool isAltHoldActive(void);
#endif
#endif // USE_WING

View file

@ -19,16 +19,5 @@
#include "io/gps.h"
extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(void);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz);
void posControlOutput(void);
bool positionControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);
bool isBelowLandingAltitude(void);
float getAutopilotThrottle(void);
bool isAutopilotInControl(void);
#include "flight/autopilot_multirotor.h"
#include "flight/autopilot_wing.h"

View file

@ -21,6 +21,9 @@
#include <math.h>
#include "platform.h"
#ifndef USE_WING
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
@ -395,3 +398,5 @@ bool isAutopilotInControl(void)
{
return !ap.sticksActive;
}
#endif // !USE_WING

View file

@ -0,0 +1,38 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include "io/gps.h"
extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(void);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz);
void posControlOutput(void);
bool positionControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);
bool isBelowLandingAltitude(void);
float getAutopilotThrottle(void);
bool isAutopilotInControl(void);
#endif // !USE_WING

View file

@ -0,0 +1,91 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include "platform.h"
#ifdef USE_WING
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/vector.h"
#include "fc/rc.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
#include "pg/autopilot.h"
#include "autopilot.h"
float autopilotAngle[RP_AXIS_COUNT];
void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz)
{
// from pos_hold.c (or other client) when initiating position hold at target location
UNUSED(initialTargetLocation);
UNUSED(taskRateHz);
}
void autopilotInit(void)
{
}
void resetAltitudeControl (void) {
}
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep)
{
UNUSED(targetAltitudeCm);
UNUSED(taskIntervalS);
UNUSED(targetAltitudeStep);
}
void setSticksActiveStatus(bool areSticksActive)
{
UNUSED(areSticksActive);
}
bool positionControl(void)
{
return false;
}
bool isBelowLandingAltitude(void)
{
return false;
}
float getAutopilotThrottle(void)
{
return 0.0f;
}
bool isAutopilotInControl(void)
{
return false;
}
#endif // USE_WING

View file

@ -0,0 +1,37 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include "io/gps.h"
extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(void);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(const gpsLocation_t *initialTargetLocation, unsigned taskRateHz);
bool positionControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);
bool isBelowLandingAltitude(void);
float getAutopilotThrottle(void);
bool isAutopilotInControl(void);
#endif // USE_WING

View file

@ -17,41 +17,5 @@
#pragma once
#include <stdbool.h>
#include "common/axis.h"
#include "pg/gps_rescue.h"
#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate
#ifdef USE_MAG
#define GPS_RESCUE_USE_MAG true
#else
#define GPS_RESCUE_USE_MAG false
#endif
typedef enum {
RESCUE_SANITY_OFF = 0,
RESCUE_SANITY_ON,
RESCUE_SANITY_FS_ONLY,
RESCUE_SANITY_COUNT
} gpsRescueSanity_e;
typedef enum {
GPS_RESCUE_ALT_MODE_MAX = 0,
GPS_RESCUE_ALT_MODE_FIXED,
GPS_RESCUE_ALT_MODE_CURRENT,
GPS_RESCUE_ALT_MODE_COUNT
} gpsRescueAltitudeMode_e;
extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void gpsRescueInit(void);
void gpsRescueUpdate(void);
float gpsRescueGetYawRate(void);
bool gpsRescueIsConfigured(void);
bool gpsRescueIsAvailable(void);
bool gpsRescueIsDisabled(void);
bool gpsRescueDisableMag(void);
float gpsRescueGetImuYawCogGain(void);
#include "flight/gps_rescue_multirotor.h"
#include "flight/gps_rescue_wing.h"

View file

@ -21,6 +21,7 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_GPS_RESCUE
#include "build/debug.h"
@ -138,7 +139,7 @@ void gpsRescueInit(void)
rescueState.intent.velocityPidCutoffModifier = 1.0f;
gain = pt1FilterGain(cutoffHz, 1.0f);
pt1FilterInit(&velocityDLpf, gain);
cutoffHz *= 4.0f;
cutoffHz *= 4.0f;
gain = pt3FilterGain(cutoffHz, taskIntervalSeconds);
pt3FilterInit(&velocityUpsampleLpf, gain);
}
@ -887,3 +888,5 @@ bool gpsRescueDisableMag(void)
}
#endif
#endif
#endif // !USE_WING

View file

@ -0,0 +1,61 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include <stdbool.h>
#include "common/axis.h"
#include "pg/gps_rescue.h"
#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate
#ifdef USE_MAG
#define GPS_RESCUE_USE_MAG true
#else
#define GPS_RESCUE_USE_MAG false
#endif
typedef enum {
RESCUE_SANITY_OFF = 0,
RESCUE_SANITY_ON,
RESCUE_SANITY_FS_ONLY,
RESCUE_SANITY_COUNT
} gpsRescueSanity_e;
typedef enum {
GPS_RESCUE_ALT_MODE_MAX = 0,
GPS_RESCUE_ALT_MODE_FIXED,
GPS_RESCUE_ALT_MODE_CURRENT,
GPS_RESCUE_ALT_MODE_COUNT
} gpsRescueAltitudeMode_e;
extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void gpsRescueInit(void);
void gpsRescueUpdate(void);
float gpsRescueGetYawRate(void);
bool gpsRescueIsConfigured(void);
bool gpsRescueIsAvailable(void);
bool gpsRescueIsDisabled(void);
bool gpsRescueDisableMag(void);
float gpsRescueGetImuYawCogGain(void);
#endif // !USE_WING

View file

@ -0,0 +1,100 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#ifdef USE_WING
#ifdef USE_GPS_RESCUE
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "config/config.h"
#include "drivers/time.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "pg/autopilot.h"
#include "sensors/acceleration.h"
#include "gps_rescue.h"
float gpsRescueAngle[RP_AXIS_COUNT] = { 0, 0 };
void gpsRescueInit(void)
{
}
void gpsRescueUpdate(void)
// runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active
{
}
float gpsRescueGetYawRate(void)
{
return 0.0f; // the control yaw value for rc.c to be used while flightMode gps_rescue is active.
}
float gpsRescueGetImuYawCogGain(void)
{
return 1.0f;
}
bool gpsRescueIsConfigured(void)
{
return false;
}
bool gpsRescueIsAvailable(void)
{
return false;
}
bool gpsRescueIsDisabled(void)
{
return true;
}
#ifdef USE_MAG
bool gpsRescueDisableMag(void)
{
return true;
}
#endif
#endif // USE_GPS_RESCUE
#endif // USE_WING

View file

@ -0,0 +1,41 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include <stdbool.h>
#include "common/axis.h"
#include "pg/gps_rescue.h"
#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate
extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void gpsRescueInit(void);
void gpsRescueUpdate(void);
float gpsRescueGetYawRate(void);
bool gpsRescueIsConfigured(void);
bool gpsRescueIsAvailable(void);
bool gpsRescueIsDisabled(void);
bool gpsRescueDisableMag(void);
float gpsRescueGetImuYawCogGain(void);
#endif // USE_WING

View file

@ -570,7 +570,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
#ifdef USE_GPS_RESCUE
angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
#endif
#ifdef USE_POSITION_HOLD
#if defined(USE_POSITION_HOLD) && !defined(USE_WING)
if (FLIGHT_MODE(POS_HOLD_MODE)) {
angleFeedforward = 0.0f; // otherwise the lag of the PT3 carries recent stick inputs into the hold
if (isAutopilotInControl()) {
@ -1127,7 +1127,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
|| FLIGHT_MODE(ALT_HOLD_MODE) // todo - check if this is needed
#endif
#ifdef USE_POSITION_HOLD
|| FLIGHT_MODE(POS_HOLD_MODE)
|| FLIGHT_MODE(POS_HOLD_MODE)
#endif
;
levelMode_e levelMode;

View file

@ -17,17 +17,5 @@
#pragma once
// #include "pg/pos_hold.h"
#ifdef USE_POSITION_HOLD
#include "common/time.h"
#include "io/gps.h"
#define POSHOLD_TASK_RATE_HZ 100 // hz
void posHoldInit(void);
void updatePosHold(timeUs_t currentTimeUs);
bool posHoldFailure(void);
#endif
#include "flight/pos_hold_multirotor.h"
#include "flight/pos_hold_wing.h"

View file

@ -17,6 +17,8 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_POSITION_HOLD
#include "math.h"
@ -55,7 +57,7 @@ void posHoldInit(void)
void posHoldCheckSticks(void)
{
// if failsafe is active, eg landing mode, don't update the original start point
// if failsafe is active, eg landing mode, don't update the original start point
if (!failsafeIsActive() && posHold.useStickAdjustment) {
const bool sticksDeflected = (getRcDeflectionAbs(FD_ROLL) > posHold.deadband) || (getRcDeflectionAbs(FD_PITCH) > posHold.deadband);
setSticksActiveStatus(sticksDeflected);
@ -65,20 +67,20 @@ void posHoldCheckSticks(void)
bool sensorsOk(void)
{
if (!STATE(GPS_FIX)) {
return false;
return false;
}
if (
#ifdef USE_MAG
!compassIsHealthy() &&
#endif
(!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) {
return false;
return false;
}
return true;
}
void updatePosHold(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
UNUSED(currentTimeUs);
if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (!posHold.isEnabled) {
resetPositionControl(&gpsSol.llh, POSHOLD_TASK_RATE_HZ); // sets target location to current location
@ -104,3 +106,5 @@ bool posHoldFailure(void) {
}
#endif // USE_POS_HOLD
#endif // !USE_WING

View file

@ -0,0 +1,37 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
// #include "pg/pos_hold.h"
#ifdef USE_POSITION_HOLD
#include "common/time.h"
#include "io/gps.h"
#define POSHOLD_TASK_RATE_HZ 100 // hz
void posHoldInit(void);
void updatePosHold(timeUs_t currentTimeUs);
bool posHoldFailure(void);
#endif // USE_POSITION_HOLD
#endif // !USE_WING

View file

@ -0,0 +1,57 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#ifdef USE_POSITION_HOLD
#include "math.h"
#include "build/debug.h"
#include "common/maths.h"
#include "config/config.h"
#include "fc/core.h"
#include "fc/runtime_config.h"
#include "fc/rc.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/position.h"
#include "rx/rx.h"
#include "sensors/compass.h"
#include "pg/pos_hold.h"
#include "pos_hold.h"
void posHoldInit(void)
{
}
void updatePosHold(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
}
bool posHoldFailure(void) {
// used only to display warning in OSD if requested but failing
return true;
}
#endif // USE_POS_HOLD
#endif // USE_WING

View file

@ -0,0 +1,37 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
// #include "pg/pos_hold.h"
#ifdef USE_POSITION_HOLD
#include "common/time.h"
#include "io/gps.h"
#define POSHOLD_TASK_RATE_HZ 100 // hz
void posHoldInit(void);
void updatePosHold(timeUs_t currentTimeUs);
bool posHoldFailure(void);
#endif
#endif // USE_WING

View file

@ -220,7 +220,8 @@ displayPort_t *displayPortMspInit(void)
return &mspDisplayPort;
}
void displayPortMspSetSerial(serialPortIdentifier_e serialPort) {
void displayPortMspSetSerial(serialPortIdentifier_e serialPort)
{
displayPortSerial = serialPort;
}

View file

@ -1588,4 +1588,16 @@ void setLedProfile(uint8_t profile)
ledStripConfigMutable()->ledstrip_profile = profile;
}
}
uint8_t getLedBrightness(void)
{
return ledStripConfig()->ledstrip_brightness;
}
void setLedBrightness(uint8_t brightness)
{
if ( brightness <= 100 ) {
ledStripConfigMutable()->ledstrip_brightness = brightness;
}
}
#endif

View file

@ -239,3 +239,5 @@ void updateRequiredOverlay(void);
uint8_t getLedProfile(void);
void setLedProfile(uint8_t profile);
uint8_t getLedBrightness(void);
void setLedBrightness(uint8_t brightness);

View file

@ -70,6 +70,9 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
#ifdef USE_VCP
SERIAL_PORT_USB_VCP,
#endif
#ifdef USE_UART0
SERIAL_PORT_UART0,
#endif
#ifdef USE_UART1
SERIAL_PORT_USART1,
#endif
@ -115,6 +118,9 @@ const char* serialPortNames[SERIAL_PORT_COUNT] = {
#ifdef USE_VCP
"VCP",
#endif
#ifdef USE_UART0
"UART0",
#endif
#ifdef USE_UART1
"UART1",
#endif
@ -156,17 +162,24 @@ const char* serialPortNames[SERIAL_PORT_COUNT] = {
#endif
};
const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000,
400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000}; // see baudRate_e
const uint32_t baudRates[BAUD_COUNT] = {
0, 9600, 19200, 38400, 57600, 115200, 230400, 250000,
400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000
}; // see baudRate_e
static serialPortConfig_t* findInPortConfigs_identifier(const serialPortConfig_t cfgs[], size_t count, serialPortIdentifier_e identifier)
{
if (identifier == SERIAL_PORT_NONE || identifier == SERIAL_PORT_ALL) {
return NULL;
}
for (unsigned i = 0; i < count; i++) {
if (cfgs[i].identifier == identifier) {
// drop const on return - wrapper function will add it back if necessary
return (serialPortConfig_t*)&cfgs[i];
}
}
return NULL;
}

View file

@ -77,18 +77,29 @@ typedef enum {
BAUD_COUNT
} baudRate_e;
extern const uint32_t baudRates[];
extern const uint32_t baudRates[BAUD_COUNT];
// serial port identifiers are now fixed, these values are used by MSP commands.
typedef enum {
SERIAL_PORT_ALL = -2,
SERIAL_PORT_NONE = -1,
// prepare for transition to SERIAL_PORT_UART0
SERIAL_PORT_UART_FIRST = 0,
SERIAL_PORT_LEGACY_START_IDENTIFIER = 0,
SERIAL_PORT_START_IDENTIFIER = 20,
SERIAL_PORT_USB_VCP = 20,
SERIAL_PORT_SOFTSERIAL_FIRST = 30,
SERIAL_PORT_SOFTSERIAL1 = SERIAL_PORT_SOFTSERIAL_FIRST,
SERIAL_PORT_SOFTSERIAL2,
SERIAL_PORT_LPUART_FIRST = 40,
SERIAL_PORT_LPUART1 = SERIAL_PORT_LPUART_FIRST,
#if SERIAL_UART_FIRST_INDEX == 0
SERIAL_PORT_UART_FIRST = 50,
SERIAL_PORT_UART0 = SERIAL_PORT_UART_FIRST,
SERIAL_PORT_USART1,
#else
SERIAL_PORT_UART_FIRST = 51,
SERIAL_PORT_USART1 = SERIAL_PORT_UART_FIRST,
#endif
SERIAL_PORT_UART1 = SERIAL_PORT_USART1,
@ -108,14 +119,6 @@ typedef enum {
SERIAL_PORT_USART10,
SERIAL_PORT_UART10 = SERIAL_PORT_USART10,
SERIAL_PORT_USB_VCP = 20,
SERIAL_PORT_SOFTSERIAL_FIRST = 30,
SERIAL_PORT_SOFTSERIAL1 = SERIAL_PORT_SOFTSERIAL_FIRST,
SERIAL_PORT_SOFTSERIAL2,
SERIAL_PORT_LPUART_FIRST = 40,
SERIAL_PORT_LPUART1 = SERIAL_PORT_LPUART_FIRST,
} serialPortIdentifier_e;
// use value from target serial port normalization

View file

@ -60,9 +60,8 @@ serialType_e serialType(serialPortIdentifier_e identifier)
}
#endif
#ifdef USE_SOFTSERIAL
if (identifier >= SERIAL_PORT_SOFTSERIAL_FIRST
&& identifier < SERIAL_PORT_SOFTSERIAL_FIRST + SERIAL_SOFTSERIAL_MAX) {
// sotserials always start from first index, without holes
if (identifier >= SERIAL_PORT_SOFTSERIAL_FIRST && identifier < SERIAL_PORT_SOFTSERIAL_FIRST + SERIAL_SOFTSERIAL_MAX) {
// sotserials always start from 1, without holes
return SERIALTYPE_SOFTSERIAL;
}
#endif

View file

@ -124,6 +124,7 @@
#include "pg/gps_rescue.h"
#include "pg/gyrodev.h"
#include "pg/motor.h"
#include "pg/pilot.h"
#include "pg/pos_hold.h"
#include "pg/rx.h"
#include "pg/rx_spi.h"
@ -1158,12 +1159,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
break;
case MSP_NAME:
{
const int nameLen = strlen(pilotConfig()->craftName);
for (int i = 0; i < nameLen; i++) {
sbufWriteU8(dst, pilotConfig()->craftName[i]);
}
}
sbufWriteString(dst, pilotConfig()->craftName);
break;
#ifdef USE_SERVOS
@ -1537,6 +1533,7 @@ case MSP_NAME:
break;
#ifdef USE_GPS_RESCUE
#ifndef USE_WING
case MSP_GPS_RESCUE:
sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM);
@ -1569,6 +1566,7 @@ case MSP_NAME:
sbufWriteU16(dst, gpsRescueConfig()->velD);
sbufWriteU16(dst, gpsRescueConfig()->yawP);
break;
#endif // !USE_WING
#endif
#endif
@ -1806,7 +1804,7 @@ case MSP_NAME:
case MSP_RC_DEADBAND:
sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
#ifdef USE_POSITION_HOLD
#if defined(USE_POSITION_HOLD) && !defined(USE_WING)
sbufWriteU8(dst, posHoldConfig()->pos_hold_deadband);
#else
sbufWriteU8(dst, 0);
@ -2610,9 +2608,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
// type byte, then length byte followed by the actual characters
sbufWriteU8(dst, textType);
sbufWriteU8(dst, textLength);
for (unsigned int i = 0; i < textLength; i++) {
sbufWriteU8(dst, textVar[i]);
}
sbufWriteData(dst, textVar, textLength);
}
break;
#ifdef USE_LED_STRIP
@ -2885,6 +2881,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#ifdef USE_GPS
#ifdef USE_GPS_RESCUE
#ifndef USE_WING
case MSP_SET_GPS_RESCUE:
gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src);
@ -2922,6 +2919,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
gpsRescueConfigMutable()->velD = sbufReadU16(src);
gpsRescueConfigMutable()->yawP = sbufReadU16(src);
break;
#endif // !USE_WING
#endif
#endif
@ -2977,7 +2975,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
case MSP_SET_RC_DEADBAND:
rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
#ifdef USE_POSITION_HOLD
#if defined(USE_POSITION_HOLD) && !defined(USE_WING)
posHoldConfigMutable()->pos_hold_deadband = sbufReadU8(src);
#else
sbufReadU8(src);
@ -3895,7 +3893,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
return MSP_RESULT_ERROR;
}
portConfig->identifier = identifier;
portConfig->functionMask = sbufReadU16(src);
portConfig->msp_baudrateIndex = sbufReadU8(src);
portConfig->gps_baudrateIndex = sbufReadU8(src);
@ -3923,7 +3920,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
return MSP_RESULT_ERROR;
}
portConfig->identifier = identifier;
portConfig->functionMask = sbufReadU32(src);
portConfig->msp_baudrateIndex = sbufReadU8(src);
portConfig->gps_baudrateIndex = sbufReadU8(src);
@ -3985,10 +3981,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#endif
case MSP_SET_NAME:
memset(pilotConfigMutable()->craftName, 0, ARRAYLEN(pilotConfig()->craftName));
for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
pilotConfigMutable()->craftName[i] = sbufReadU8(src);
}
memset(pilotConfigMutable()->craftName, 0, sizeof(pilotConfigMutable()->craftName));
sbufReadData(src, pilotConfigMutable()->craftName, MIN(ARRAYLEN(pilotConfigMutable()->craftName) - 1, dataSize));
#ifdef USE_OSD
osdAnalyzeActiveElements();
#endif
@ -4068,35 +4062,51 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
case MSP2_SET_TEXT:
{
// type byte, then length byte followed by the actual characters
const uint8_t textType = sbufReadU8(src);
const unsigned textType = sbufReadU8(src);
char* textVar;
const uint8_t textLength = MIN(MAX_NAME_LENGTH, sbufReadU8(src));
unsigned textSpace;
switch (textType) {
case MSP2TEXT_PILOT_NAME:
textVar = pilotConfigMutable()->pilotName;
textSpace = sizeof(pilotConfigMutable()->pilotName) - 1;
break;
case MSP2TEXT_CRAFT_NAME:
textVar = pilotConfigMutable()->craftName;
textSpace = sizeof(pilotConfigMutable()->craftName) - 1;
break;
case MSP2TEXT_PID_PROFILE_NAME:
textVar = currentPidProfile->profileName;
textSpace = sizeof(currentPidProfile->profileName) - 1;
break;
case MSP2TEXT_RATE_PROFILE_NAME:
textVar = currentControlRateProfile->profileName;
textSpace = sizeof(currentControlRateProfile->profileName) - 1;
break;
case MSP2TEXT_CUSTOM_MSG_0:
case MSP2TEXT_CUSTOM_MSG_0 + 1:
case MSP2TEXT_CUSTOM_MSG_0 + 2:
case MSP2TEXT_CUSTOM_MSG_0 + 3: {
unsigned msgIdx = textType - MSP2TEXT_CUSTOM_MSG_0;
if (msgIdx < OSD_CUSTOM_MSG_COUNT) {
textVar = pilotConfigMutable()->message[msgIdx];
textSpace = sizeof(pilotConfigMutable()->message[msgIdx]) - 1;
} else {
return MSP_RESULT_ERROR;
}
break;
}
default:
return MSP_RESULT_ERROR;
}
const unsigned textLength = MIN(textSpace, sbufReadU8(src));
memset(textVar, 0, strlen(textVar));
for (unsigned int i = 0; i < textLength; i++) {
textVar[i] = sbufReadU8(src);
}
sbufReadData(src, textVar, textLength);
#ifdef USE_OSD
if (textType == MSP2TEXT_PILOT_NAME || textType == MSP2TEXT_CRAFT_NAME) {

View file

@ -38,3 +38,6 @@
#define MSP2TEXT_RATE_PROFILE_NAME 4
#define MSP2TEXT_BUILDKEY 5
#define MSP2TEXT_RELEASENAME 6
#define MSP2TEXT_CUSTOM_MSG_0 7 // CUSTOM_MSG_MAX_NUM entries are allocated
#define CUSTOM_MSG_MAX_NUM 4
// next new variable type must be >= MSP2TEXT_CUSTOM_MSG_0 + CUSTOM_MSG_MAX_NUM (11)

View file

@ -190,6 +190,10 @@ typedef enum {
OSD_GPS_LAP_TIME_PREVIOUS,
OSD_GPS_LAP_TIME_BEST3,
OSD_DEBUG2,
OSD_CUSTOM_MSG0,
OSD_CUSTOM_MSG1,
OSD_CUSTOM_MSG2,
OSD_CUSTOM_MSG3,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

View file

@ -161,6 +161,7 @@
#include "osd/osd_warnings.h"
#include "pg/motor.h"
#include "pg/pilot.h"
#include "pg/stats.h"
#include "rx/rx.h"
@ -810,6 +811,18 @@ static void osdElementCompassBar(osdElementParms_t *element)
element->buff[9] = 0;
}
//display custom message from MSPv2
static void osdElementCustomMsg(osdElementParms_t *element)
{
int msgIndex = element->item - OSD_CUSTOM_MSG0;
if (msgIndex < 0 || msgIndex >= OSD_CUSTOM_MSG_COUNT || pilotConfig()->message[msgIndex][0] == '\0') {
tfp_sprintf(element->buff, "CUSTOM_MSG%d", msgIndex + 1);
} else {
strncpy(element->buff, pilotConfig()->message[msgIndex], MAX_NAME_LENGTH);
element->buff[MAX_NAME_LENGTH] = 0; // terminate maximum-length string
}
}
#ifdef USE_ADC_INTERNAL
static void osdElementCoreTemperature(osdElementParms_t *element)
{
@ -1804,6 +1817,10 @@ static const uint8_t osdElementDisplayOrder[] = {
OSD_MAH_DRAWN,
OSD_WATT_HOURS_DRAWN,
OSD_CRAFT_NAME,
OSD_CUSTOM_MSG0,
OSD_CUSTOM_MSG1,
OSD_CUSTOM_MSG2,
OSD_CUSTOM_MSG3,
OSD_ALTITUDE,
OSD_ROLL_PIDS,
OSD_PITCH_PIDS,
@ -1902,6 +1919,10 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_ITEM_TIMER_2] = osdElementTimer,
[OSD_FLYMODE] = osdElementFlymode,
[OSD_CRAFT_NAME] = NULL, // only has background
[OSD_CUSTOM_MSG0] = osdElementCustomMsg,
[OSD_CUSTOM_MSG1] = osdElementCustomMsg,
[OSD_CUSTOM_MSG2] = osdElementCustomMsg,
[OSD_CUSTOM_MSG3] = osdElementCustomMsg,
[OSD_THROTTLE_POS] = osdElementThrottlePosition,
#ifdef USE_VTX_COMMON
[OSD_VTX_CHANNEL] = osdElementVtxChannel,

View file

@ -21,13 +21,5 @@
#pragma once
#include <stdint.h>
#include "pg/pg.h"
typedef struct altHoldConfig_s {
uint8_t alt_hold_adjust_rate;
uint8_t alt_hold_deadband;
} altHoldConfig_t;
PG_DECLARE(altHoldConfig_t, altHoldConfig);
#include "pg/alt_hold_multirotor.h"
#include "pg/alt_hold_wing.h"

View file

@ -21,6 +21,8 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_ALTITUDE_HOLD
#include "flight/alt_hold.h"
@ -37,3 +39,5 @@ PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
.alt_hold_deadband = 20, // throttle deadband in percent of stick travel
);
#endif
#endif // USE_WING

View file

@ -0,0 +1,37 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct altHoldConfig_s {
uint8_t alt_hold_adjust_rate;
uint8_t alt_hold_deadband;
} altHoldConfig_t;
PG_DECLARE(altHoldConfig_t, altHoldConfig);
#endif // !USE_WING

View file

@ -0,0 +1,41 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#ifdef USE_ALTITUDE_HOLD
#include "flight/alt_hold.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "alt_hold.h"
PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4);
PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
);
#endif
#endif // USE_WING

View file

@ -0,0 +1,36 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct altHoldConfig_s {
uint8_t dummy;
} altHoldConfig_t;
PG_DECLARE(altHoldConfig_t, altHoldConfig);
#endif // USE_WING

View file

@ -21,26 +21,5 @@
#pragma once
#include <stdint.h>
#include "pg/pg.h"
typedef struct apConfig_s {
uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
uint16_t hover_throttle; // value used at the start of a rescue or position hold
uint16_t throttle_min;
uint16_t throttle_max;
uint8_t altitude_P;
uint8_t altitude_I;
uint8_t altitude_D;
uint8_t altitude_F;
uint8_t position_P;
uint8_t position_I;
uint8_t position_D;
uint8_t position_A;
uint8_t position_cutoff;
uint8_t max_angle;
} apConfig_t;
PG_DECLARE(apConfig_t, apConfig);
#include "pg/autopilot_multirotor.h"
#include "pg/autopilot_wing.h"

View file

@ -21,6 +21,8 @@
#include "platform.h"
#ifndef USE_WING
#include "flight/autopilot.h"
#include "pg/pg.h"
@ -46,3 +48,5 @@ PG_RESET_TEMPLATE(apConfig_t, apConfig,
.position_cutoff = 80,
.max_angle = 50,
);
#endif // !USE_WING

View file

@ -0,0 +1,49 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct apConfig_s {
uint8_t landing_altitude_m; // altitude below which landing behaviours can change, metres
uint16_t hover_throttle; // value used at the start of a rescue or position hold
uint16_t throttle_min;
uint16_t throttle_max;
uint8_t altitude_P;
uint8_t altitude_I;
uint8_t altitude_D;
uint8_t altitude_F;
uint8_t position_P;
uint8_t position_I;
uint8_t position_D;
uint8_t position_A;
uint8_t position_cutoff;
uint8_t max_angle;
} apConfig_t;
PG_DECLARE(apConfig_t, apConfig);
#endif // !USE_WING

View file

@ -0,0 +1,38 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#include "flight/autopilot.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "autopilot.h"
PG_REGISTER_WITH_RESET_TEMPLATE(apConfig_t, apConfig, PG_AUTOPILOT, 2);
PG_RESET_TEMPLATE(apConfig_t, apConfig,
);
#endif // USE_WING

View file

@ -0,0 +1,36 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct apConfig_s {
uint8_t dummy;
} apConfig_t;
PG_DECLARE(apConfig_t, apConfig);
#endif // USE_WING

View file

@ -1,50 +1,21 @@
/*
* This file is part of Cleanflight and Betaflight.
* This file is part of Betaflight.
*
* Cleanflight and Betaflight are 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.
* Betaflight 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 and Betaflight are distributed in the hope that they
* 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.
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include "pg/pg.h"
typedef struct gpsRescue_s {
uint16_t maxRescueAngle; // degrees
uint16_t returnAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t groundSpeedCmS; // centimeters per second
uint8_t yawP;
uint8_t minSats;
uint8_t velP, velI, velD;
uint16_t minStartDistM; // meters
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
uint8_t useMag;
uint8_t altitudeMode;
uint16_t ascendRate;
uint16_t descendRate;
uint16_t initialClimbM; // meters
uint8_t rollMix;
uint8_t disarmThreshold;
uint8_t pitchCutoffHz;
uint8_t imuYawGain;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
#include "pg/gps_rescue_multirotor.h"
#include "pg/gps_rescue_wing.h"

View file

@ -20,6 +20,8 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_GPS_RESCUE
#include "flight/gps_rescue.h"
@ -62,3 +64,5 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
);
#endif // USE_GPS_RESCUE
#endif // !USE_WING

View file

@ -0,0 +1,54 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are 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.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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
#ifndef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct gpsRescue_s {
uint16_t maxRescueAngle; // degrees
uint16_t returnAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t groundSpeedCmS; // centimeters per second
uint8_t yawP;
uint8_t minSats;
uint8_t velP, velI, velD;
uint16_t minStartDistM; // meters
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
uint8_t useMag;
uint8_t altitudeMode;
uint16_t ascendRate;
uint16_t descendRate;
uint16_t initialClimbM; // meters
uint8_t rollMix;
uint8_t disarmThreshold;
uint8_t pitchCutoffHz;
uint8_t imuYawGain;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
#endif // !USE_WING

View file

@ -0,0 +1,40 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#ifdef USE_GPS_RESCUE
#include "flight/gps_rescue.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "gps_rescue.h"
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 7);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.allowArmingWithoutFix = false,
.minSats = 8,
);
#endif // USE_GPS_RESCUE
#endif // USE_WING

View file

@ -0,0 +1,33 @@
/*
* This file is part of Betaflight.
*
* Betaflight 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.
*
* Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct gpsRescue_s {
uint8_t allowArmingWithoutFix;
uint8_t minSats;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
#endif // USE_WING

38
src/main/pg/pilot.c Normal file
View file

@ -0,0 +1,38 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <string.h>
#include "platform.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pilot.h"
PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 2);
PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig,
.craftName = { 0 },
.pilotName = { 0 },
.message = { {0} },
);

35
src/main/pg/pilot.h Normal file
View file

@ -0,0 +1,35 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "pg/pg.h"
#define MAX_NAME_LENGTH 16u
#define OSD_CUSTOM_MSG_COUNT 4
typedef struct pilotConfig_s {
char craftName[MAX_NAME_LENGTH + 1];
char pilotName[MAX_NAME_LENGTH + 1];
char message[OSD_CUSTOM_MSG_COUNT][MAX_NAME_LENGTH + 1];
} pilotConfig_t;
PG_DECLARE(pilotConfig_t, pilotConfig);

View file

@ -21,13 +21,5 @@
#pragma once
#include <stdint.h>
#include "pg/pg.h"
typedef struct posHoldConfig_s {
bool pos_hold_without_mag;
uint8_t pos_hold_deadband;
} posHoldConfig_t;
PG_DECLARE(posHoldConfig_t, posHoldConfig);
#include "pg/pos_hold_multirotor.h"
#include "pg/pos_hold_wing.h"

View file

@ -21,6 +21,8 @@
#include "platform.h"
#ifndef USE_WING
#ifdef USE_POSITION_HOLD
#include "flight/pos_hold.h"
@ -37,3 +39,5 @@ PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
.pos_hold_deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks
);
#endif
#endif // !USE_WING

View file

@ -0,0 +1,37 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifndef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct posHoldConfig_s {
bool pos_hold_without_mag;
uint8_t pos_hold_deadband;
} posHoldConfig_t;
PG_DECLARE(posHoldConfig_t, posHoldConfig);
#endif // !USE_WING

View file

@ -0,0 +1,41 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_WING
#ifdef USE_POSITION_HOLD
#include "flight/pos_hold.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pos_hold.h"
PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0);
PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
);
#endif
#endif // USE_WING

View file

@ -0,0 +1,36 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef USE_WING
#include <stdint.h>
#include "pg/pg.h"
typedef struct posHoldConfig_s {
uint8_t dummy;
} posHoldConfig_t;
PG_DECLARE(posHoldConfig_t, posHoldConfig);
#endif // !USE_WING

View file

@ -46,6 +46,9 @@ typedef struct uartDmaopt_s {
} uartDmaopt_t;
static const uartDmaopt_t uartDmaopt[] = {
#ifdef USE_UART0
{ SERIAL_PORT_UART0, UART0_TX_DMA_OPT, UART0_RX_DMA_OPT },
#endif
#ifdef USE_UART1
{ SERIAL_PORT_USART1, UART1_TX_DMA_OPT, UART1_RX_DMA_OPT },
#endif

View file

@ -211,6 +211,15 @@
#endif
#endif
#ifdef USE_UART0
#ifndef UART0_TX_DMA_OPT
#define UART0_TX_DMA_OPT (DMA_OPT_UNUSED)
#endif
#ifndef UART0_RX_DMA_OPT
#define UART0_RX_DMA_OPT (DMA_OPT_UNUSED)
#endif
#endif
#ifdef USE_UART1
#ifndef UART1_TX_DMA_OPT
#define UART1_TX_DMA_OPT (DMA_OPT_UNUSED)

View file

@ -555,14 +555,20 @@
#undef USED_TIMERS
#endif
#if !defined(USE_RANGEFINDER)
#undef USE_RANGEFINDER_HCSR04
#undef USE_RANGEFINDER_SRF10
#undef USE_RANGEFINDER_HCSR04_I2C
#undef USE_RANGEFINDER_VL53L0X
#undef USE_RANGEFINDER_UIB
#undef USE_RANGEFINDER_TF
#if defined(USE_OPTICALFLOW_MT)
#ifndef USE_RANGEFINDER_MT
#define USE_RANGEFINDER_MT
#endif
#ifndef USE_OPTICALFLOW
#define USE_OPTICALFLOW
#endif
#endif // USE_OPTICALFLOW_MT
#if defined(USE_RANGEFINDER_HCSR04) || defined(USE_RANGEFINDER_SRF10) || defined(USE_RANGEFINDER_HCSR04_I2C) || defined(USE_RANGEFINDER_VL53L0X) || defined(USE_RANGEFINDER_UIB) || defined(USE_RANGEFINDER_TF) || defined(USE_RANGEFINDER_MT)
#ifndef USE_RANGEFINDER
#define USE_RANGEFINDER
#endif
#endif // USE_RANGEFINDER_XXX
#ifndef USE_GPS_RESCUE
#undef USE_CMS_GPS_RESCUE_MENU

View file

@ -270,8 +270,9 @@
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define USE_RANGEFINDER_TF
#define USE_OPTICALFLOW_MT
#endif // TARGET_FLASH_SIZE > 512
#endif // TARGET_FLASH_SIZE >= 1024
#endif // !defined(CLOUD_BUILD)
@ -482,3 +483,7 @@
#undef USE_RUNAWAY_TAKEOFF
#endif // USE_WING
#if defined(USE_POSITION_HOLD) && !defined(USE_GPS)
#error "USE_POSITION_HOLD requires USE_GPS to be defined"
#endif

View file

@ -0,0 +1,232 @@
/*
* This file is part of Betaflight.
*
* Betaflight is 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.
*
* Betaflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#ifdef USE_TIMER
#include "common/utils.h"
#include "drivers/dma.h"
#include "drivers/io.h"
#include "timer_def.h"
#include "stm32h5xx.h"
#include "drivers/rcc.h"
#include "drivers/timer.h"
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn},
{ .TIMx = TIM2, .rcc = RCC_APB1L(TIM2), .inputIrq = TIM2_IRQn},
{ .TIMx = TIM3, .rcc = RCC_APB1L(TIM3), .inputIrq = TIM3_IRQn},
{ .TIMx = TIM4, .rcc = RCC_APB1L(TIM4), .inputIrq = TIM4_IRQn},
{ .TIMx = TIM5, .rcc = RCC_APB1L(TIM5), .inputIrq = TIM5_IRQn},
{ .TIMx = TIM6, .rcc = RCC_APB1L(TIM6), .inputIrq = TIM6_DAC_IRQn},
{ .TIMx = TIM7, .rcc = RCC_APB1L(TIM7), .inputIrq = TIM7_IRQn},
{ .TIMx = TIM8, .rcc = RCC_APB2(TIM8), .inputIrq = TIM8_CC_IRQn},
{ .TIMx = TIM12, .rcc = RCC_APB1L(TIM12), .inputIrq = TIM8_BRK_TIM12_IRQn},
{ .TIMx = TIM13, .rcc = RCC_APB1L(TIM13), .inputIrq = TIM8_UP_TIM13_IRQn},
{ .TIMx = TIM14, .rcc = RCC_APB1L(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn},
{ .TIMx = TIM15, .rcc = RCC_APB2(TIM15), .inputIrq = TIM15_IRQn},
{ .TIMx = TIM16, .rcc = RCC_APB2(TIM16), .inputIrq = TIM16_IRQn},
{ .TIMx = TIM17, .rcc = RCC_APB2(TIM17), .inputIrq = TIM17_IRQn},
};
#if defined(USE_TIMER_MGMT)
const timerHardware_t fullTimerHardware[FULL_TIMER_CHANNEL_COUNT] = {
// Auto-generated from 'timer_def.h'
// Port A
DEF_TIM(TIM2, CH1, PA0, 0, 0, 0), // AF1
DEF_TIM(TIM2, CH2, PA1, 0, 0, 0),
DEF_TIM(TIM2, CH3, PA2, 0, 0, 0),
DEF_TIM(TIM2, CH4, PA3, 0, 0, 0),
DEF_TIM(TIM2, CH1, PA5, 0, 0, 0),
DEF_TIM(TIM1, CH1N, PA7, 0, 0, 0),
DEF_TIM(TIM1, CH1, PA8, 0, 0, 0),
DEF_TIM(TIM1, CH2, PA9, 0, 0, 0),
DEF_TIM(TIM1, CH3, PA10, 0, 0, 0),
DEF_TIM(TIM1, CH4, PA11, 0, 0, 0),
DEF_TIM(TIM2, CH1, PA15, 0, 0, 0),
DEF_TIM(TIM5, CH1, PA0, 0, 0, 0), // AF2
DEF_TIM(TIM5, CH2, PA1, 0, 0, 0),
DEF_TIM(TIM5, CH3, PA2, 0, 0, 0),
DEF_TIM(TIM5, CH4, PA3, 0, 0, 0),
DEF_TIM(TIM3, CH1, PA6, 0, 0, 0),
DEF_TIM(TIM3, CH2, PA7, 0, 0, 0),
DEF_TIM(TIM8, CH1N, PA5, 0, 0, 0), // AF3
DEF_TIM(TIM8, CH1N, PA7, 0, 0, 0),
DEF_TIM(TIM13, CH1, PA6, 0, 0, 0), // AF9
DEF_TIM(TIM14, CH1, PA7, 0, 0, 0),
DEF_TIM(TIM15, CH1N, PA1, 0, 0, 0), // AF4
DEF_TIM(TIM15, CH1, PA2, 0, 0, 0),
DEF_TIM(TIM15, CH2, PA3, 0, 0, 0),
// Port B
DEF_TIM(TIM1, CH2N, PB0, 0, 0, 0), // AF1
DEF_TIM(TIM1, CH3N, PB1, 0, 0, 0),
DEF_TIM(TIM2, CH2, PB3, 0, 0, 0),
DEF_TIM(TIM16, CH1N, PB6, 0, 0, 0),
DEF_TIM(TIM17, CH1N, PB7, 0, 0, 0),
DEF_TIM(TIM16, CH1, PB8, 0, 0, 0),
DEF_TIM(TIM17, CH1, PB9, 0, 0, 0),
DEF_TIM(TIM2, CH3, PB10, 0, 0, 0),
DEF_TIM(TIM2, CH4, PB11, 0, 0, 0),
DEF_TIM(TIM1, CH1N, PB13, 0, 0, 0),
DEF_TIM(TIM1, CH2N, PB14, 0, 0, 0),
DEF_TIM(TIM1, CH3N, PB15, 0, 0, 0),
DEF_TIM(TIM3, CH3, PB0, 0, 0, 0), // AF2
DEF_TIM(TIM3, CH4, PB1, 0, 0, 0),
DEF_TIM(TIM3, CH1, PB4, 0, 0, 0),
DEF_TIM(TIM3, CH2, PB5, 0, 0, 0),
DEF_TIM(TIM4, CH1, PB6, 0, 0, 0),
DEF_TIM(TIM4, CH2, PB7, 0, 0, 0),
DEF_TIM(TIM4, CH3, PB8, 0, 0, 0),
DEF_TIM(TIM4, CH4, PB9, 0, 0, 0),
// DEF_TIM(TIM12, CH1, PB14, 0, 0, 0), // AF9 (SDMMC2_D0)
// DEF_TIM(TIM12, CH2, PB15, 0, 0, 0), // AF9 (SDMMC2_D1)
// Port C
DEF_TIM(TIM3, CH1, PC6, 0, 0, 0), // AF2
DEF_TIM(TIM3, CH2, PC7, 0, 0, 0),
DEF_TIM(TIM3, CH3, PC8, 0, 0, 0),
DEF_TIM(TIM3, CH4, PC9, 0, 0, 0),
DEF_TIM(TIM8, CH1, PC6, 0, 0, 0), // AF3
DEF_TIM(TIM8, CH2, PC7, 0, 0, 0),
DEF_TIM(TIM8, CH3, PC8, 0, 0, 0),
DEF_TIM(TIM8, CH4, PC9, 0, 0, 0),
// Port D
DEF_TIM(TIM1, CH4N, PD5, 0, 0, 0), // AF1 (ADDED)
DEF_TIM(TIM4, CH1, PD12, 0, 0, 0), // AF2
DEF_TIM(TIM4, CH2, PD13, 0, 0, 0),
DEF_TIM(TIM4, CH3, PD14, 0, 0, 0),
DEF_TIM(TIM4, CH4, PD15, 0, 0, 0),
// Port E
DEF_TIM(TIM1, CH1N, PE8, 0, 0, 0), // AF1
DEF_TIM(TIM1, CH1, PE9, 0, 0, 0),
DEF_TIM(TIM1, CH2N, PE10, 0, 0, 0),
DEF_TIM(TIM1, CH2, PE11, 0, 0, 0),
DEF_TIM(TIM1, CH3N, PE12, 0, 0, 0),
DEF_TIM(TIM1, CH3, PE13, 0, 0, 0),
DEF_TIM(TIM1, CH4, PE14, 0, 0, 0),
DEF_TIM(TIM1, CH4N, PE15, 0, 0, 0), // AF3 (ADDED)
DEF_TIM(TIM15, CH1N, PE4, 0, 0, 0), // AF4
DEF_TIM(TIM15, CH1, PE5, 0, 0, 0),
DEF_TIM(TIM15, CH2, PE6, 0, 0, 0),
// Port F
DEF_TIM(TIM16, CH1, PF6, 0, 0, 0), // AF1
DEF_TIM(TIM17, CH1, PF7, 0, 0, 0),
DEF_TIM(TIM16, CH1N, PF8, 0, 0, 0), // AF3
DEF_TIM(TIM17, CH1N, PF9, 0, 0, 0),
DEF_TIM(TIM13, CH1, PF8, 0, 0, 0), // AF9 (WAS TIM13_CH1N)
DEF_TIM(TIM14, CH1, PF9, 0, 0, 0), // AF9 (WAS TIM14_CH1N)
// Port H
DEF_TIM(TIM1, CHN3, PH6, 0, 0, 0), // AF1 (ADDED)
DEF_TIM(TIM1, CH3, PH7, 0, 0, 0),
DEF_TIM(TIM1, CHN2, PH8, 0, 0, 0),
DEF_TIM(TIM1, CH2, PH9, 0, 0, 0),
DEF_TIM(TIM1, CHN1, PH10, 0, 0, 0),
DEF_TIM(TIM1, CH1, PH11, 0, 0, 0),
DEF_TIM(TIM12, CH1, PH6, 0, 0, 0), // AF2 (ADDED)
DEF_TIM(TIM12, CH2, PH9, 0, 0, 0),
DEF_TIM(TIM5, CH1, PH10, 0, 0, 0),
DEF_TIM(TIM5, CH2, PH11, 0, 0, 0),
DEF_TIM(TIM5, CH3, PH12, 0, 0, 0),
DEF_TIM(TIM8, CH1, PH6, 0, 0, 0), // AF3 (ADDED)
DEF_TIM(TIM8, CH1N, PH7, 0, 0, 0),
DEF_TIM(TIM8, CH2, PH8, 0, 0, 0),
DEF_TIM(TIM8, CH2N, PH9, 0, 0, 0),
DEF_TIM(TIM8, CH3, PH10, 0, 0, 0),
DEF_TIM(TIM8, CH3N, PH11, 0, 0, 0),
DEF_TIM(TIM8, CH1N, PH13, 0, 0, 0),
DEF_TIM(TIM8, CH2N, PH14, 0, 0, 0),
DEF_TIM(TIM8, CH3N, PH15, 0, 0, 0),
// Port I
DEF_TIM(TIM5, CH4, PI0, 0, 0, 0), // AF2 (ADDED)
DEF_TIM(TIM8, CH4, PI2, 0, 0, 0), // AF3 (ADDED)
DEF_TIM(TIM8, CH1, PI5, 0, 0, 0),
DEF_TIM(TIM8, CH2, PI6, 0, 0, 0),
DEF_TIM(TIM8, CH3, PI7, 0, 0, 0),
};
#endif
uint32_t timerClock(const TIM_TypeDef *tim)
{
int timpre;
uint32_t pclk;
uint32_t ppre;
// This function is used to calculate the timer clock frequency.
// RM0481 (Rev 3) Table
RCC_ClkInitTypeDef clkConfig, uint32_t fLatency
HAL_RCC_GetClockConfig(&clkConfig, &fLatency);
if ((uintptr_t)tim >= APB2PERIPH_BASE ) { // APB2
pclk = HAL_RCC_GetPCLK2Freq();
ppre = clkConfig.APB2CLKDivider;
} else { // all other timers are on APB1
pclk = HAL_RCC_GetPCLK1Freq();
ppre = clkConfig.APB1CLKDivider;
}
timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0;
#define PC(m) (0x80 | (m)) // multiply pclk
#define HC(m) (0x00 | (m)) // multiply hclk
static const uint8_t timpreTab[2][8] = { // see RM0481 TIMPRE: timers clocks prescaler selection
// 1 1 1 1 2 4 8 16
{ HC(1), HC(1), HC(1), HC(1), HC(1), PC(2), PC(2), PC(2) }, // TIMPRE = 0
{ PC(2), PC(2), PC(2), PC(2), PC(2), PC(2), PC(4), PC(4) } // TIMPRE = 1
}
#undef PC
#undef HC
int flagMult = timpreTab[timpre][ppre];
if (flagMult & 0x80) { // PCLK based
return pclk * (flagMult & 0x7f);
} else {
return HAL_RCC_GetHCLKFreq() * (flagMult & 0x7f);
}
return pclk * periphToKernel[timpre][ppre];
}
#endif

View file

@ -37,8 +37,8 @@ alignsensor_unittest_SRC := \
$(USER_DIR)/common/vector.c
althold_unittest_SRC := \
$(USER_DIR)/flight/alt_hold.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/flight/alt_hold_multirotor.c \
$(USER_DIR)/flight/autopilot_multirotor.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/common/vector.c \
$(USER_DIR)/common/filter.c \
@ -56,8 +56,8 @@ arming_prevention_unittest_SRC := \
$(USER_DIR)/fc/rc_controls.c \
$(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/autopilot.c \
$(USER_DIR)/flight/gps_rescue.c
$(USER_DIR)/flight/autopilot_multirotor.c \
$(USER_DIR)/flight/gps_rescue_multirotor.c
arming_prevention_unittest_DEFINES := \
USE_GPS_RESCUE=

View file

@ -53,6 +53,7 @@ extern "C" {
#include "pg/pg_ids.h"
#include "pg/beeper.h"
#include "pg/gps.h"
#include "pg/pilot.h"
#include "pg/rx.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
@ -62,7 +63,7 @@ extern "C" {
void cliSet(const char *cmdName, char *cmdline);
int cliGetSettingIndex(char *name, uint8_t length);
void *cliGetValuePointer(const clivalue_t *value);
const clivalue_t valueTable[] = {
{ .name = "array_unit_test", .type = VAR_INT8 | MODE_ARRAY | MASTER_VALUE, .config = { .array = { .length = 3}}, .pgn = PG_RESERVED_FOR_TESTING_1, .offset = 0 },
{ .name = "str_unit_test", .type = VAR_UINT8 | MODE_STRING | MASTER_VALUE, .config = { .string = { 0, 16, 0 }}, .pgn = PG_RESERVED_FOR_TESTING_1, .offset = 0 },
@ -131,7 +132,7 @@ TEST(CLIUnittest, TestCliSetArray)
TEST(CLIUnittest, TestCliSetStringNoFlags)
{
char *str = (char *)"str_unit_test = SAMPLE";
char *str = (char *)"str_unit_test = SAMPLE";
cliSet("", str);
const uint16_t index = cliGetSettingIndex(str, 13);
@ -159,8 +160,8 @@ TEST(CLIUnittest, TestCliSetStringNoFlags)
TEST(CLIUnittest, TestCliSetStringWriteOnce)
{
char *str1 = (char *)"wos_unit_test = SAMPLE";
char *str2 = (char *)"wos_unit_test = ELPMAS";
char *str1 = (char *)"wos_unit_test = SAMPLE";
char *str2 = (char *)"wos_unit_test = ELPMAS";
cliSet("", str1);
const uint16_t index = cliGetSettingIndex(str1, 13);
@ -369,6 +370,19 @@ void generateLedConfig(ledConfig_t *, char *, size_t) {}
void serialSetCtrlLineStateDtrPin(serialPort_t *, ioTag_t ) {}
void serialSetCtrlLineState(serialPort_t *, uint16_t ) {}
serialPortIdentifier_e findSerialPortByName(const char* portName, int (*cmp)(const char *portName, const char *candidate))
{
UNUSED(portName);
UNUSED(cmp);
return SERIAL_PORT_NONE;
}
const char* serialName(serialPortIdentifier_e identifier, const char* notFound)
{
UNUSED(identifier);
return notFound;
}
//void serialSetBaudRateCb(serialPort_t *, void (*)(serialPort_t *context, uint32_t baud), serialPort_t *) {}
void rescheduleTask(taskId_e, timeDelta_t){}
void schedulerSetNextStateTime(timeDelta_t ){}

View file

@ -61,6 +61,7 @@ extern "C" {
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/pilot.h"
#include "pg/rx.h"
#include "rx/rx.h"

View file

@ -58,6 +58,7 @@ extern "C" {
#include "pg/gps_rescue.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/pilot.h"
#include "pg/rx.h"
#include "sensors/acceleration.h"

View file

@ -654,6 +654,8 @@ bool isTryingToArm(void) { return false; }
void resetTryingToArm(void) {}
void setLedProfile(uint8_t profile) { UNUSED(profile); }
uint8_t getLedProfile(void) { return 0; }
uint8_t getLedBrightness(void) { return 50; }
void setLedBrightness(uint8_t brightness) { UNUSED(brightness); }
void compassStartCalibration(void) {}
void pinioBoxTaskControl(void) {}
void schedulerIgnoreTaskExecTime(void) {}