Skip to content

Commit

Permalink
Separate autopilot multirotor from autopilot wing - dummy files for w…
Browse files Browse the repository at this point in the history
…ing (betaflight#14108)

* separate autopilot multirotor from autopilot wing - dummy files for wing

* fix for lisence header for new files

* filling empty PG structures with uint8_t dummy
  • Loading branch information
limonspb authored Dec 30, 2024
1 parent 2a50634 commit ef81595
Show file tree
Hide file tree
Showing 48 changed files with 1,345 additions and 194 deletions.
30 changes: 20 additions & 10 deletions mk/source.mk
Original file line number Diff line number Diff line change
@@ -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 \
Expand All @@ -15,7 +17,8 @@ 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 \
Expand All @@ -26,7 +29,8 @@ PG_SRC = \
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 \
Expand Down Expand Up @@ -154,19 +158,23 @@ 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 \
flight/mixer_tricopter.c \
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 \
Expand Down Expand Up @@ -210,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 \
Expand Down Expand Up @@ -513,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 \
Expand Down
19 changes: 13 additions & 6 deletions src/main/blackbox/blackbox.c
Original file line number Diff line number Diff line change
Expand Up @@ -745,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);
}
Expand Down Expand Up @@ -930,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);
}
Expand Down Expand Up @@ -1244,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]);
Expand Down Expand Up @@ -1695,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);
Expand All @@ -1709,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);
Expand Down Expand Up @@ -1785,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)
Expand All @@ -1809,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
Expand Down
12 changes: 10 additions & 2 deletions src/main/cli/settings.c
Original file line number Diff line number Diff line change
Expand Up @@ -1074,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) },
Expand Down Expand Up @@ -1103,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
Expand All @@ -1119,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) },
Expand Down Expand Up @@ -1867,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) },
Expand All @@ -1881,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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@

#include "platform.h"

#ifndef USE_WING

#ifdef USE_CMS_GPS_RESCUE_MENU

#include "cli/settings.h"
Expand Down Expand Up @@ -242,3 +244,5 @@ CMS_Menu cmsx_menuGpsRescue = {
};

#endif

#endif // !USE_WING
114 changes: 114 additions & 0 deletions src/main/cms/cms_menu_gps_rescue_wing.c
Original file line number Diff line number Diff line change
@@ -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
14 changes: 2 additions & 12 deletions src/main/flight/alt_hold.h
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@
*/

#include "platform.h"

#ifndef USE_WING

#include "math.h"

#ifdef USE_ALTITUDE_HOLD
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -154,3 +157,5 @@ bool isAltHoldActive(void) {
return altHold.isActive;
}
#endif

#endif // !USE_WING
Loading

0 comments on commit ef81595

Please sign in to comment.