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

Merge pull request #3159 from iNavFlight/dzikuvx-mr-cruise-experiments

Browse source 
Multirotor braking mode
This commit is contained in:
Paweł Spychalski 2018-10-06 15:51:02 +02:00 • committed by GitHub
commit 8afa15c9bc
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 246 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

26
src/main/fc/fc_msp.c
View file

@ -1357,6 +1357,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, timerHardware[i].usageFlags);
break;
case MSP2_INAV_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
sbufWriteU16(dst, navConfig()->mc.braking_disengage_speed);
sbufWriteU16(dst, navConfig()->mc.braking_timeout);
sbufWriteU8(dst, navConfig()->mc.braking_boost_factor);
sbufWriteU16(dst, navConfig()->mc.braking_boost_timeout);
sbufWriteU16(dst, navConfig()->mc.braking_boost_speed_threshold);
sbufWriteU16(dst, navConfig()->mc.braking_boost_disengage_speed);
sbufWriteU8(dst, navConfig()->mc.braking_bank_angle);
#endif
break;
default:
return false;
}
@ -2530,6 +2543,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
#endif
case MSP2_INAV_SET_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
navConfigMutable()->mc.braking_speed_threshold = sbufReadU16(src);
navConfigMutable()->mc.braking_disengage_speed = sbufReadU16(src);
navConfigMutable()->mc.braking_timeout = sbufReadU16(src);
navConfigMutable()->mc.braking_boost_factor = sbufReadU8(src);
navConfigMutable()->mc.braking_boost_timeout = sbufReadU16(src);
navConfigMutable()->mc.braking_boost_speed_threshold = sbufReadU16(src);
navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src);
navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src);
#endif
break;
case MSP2_INAV_SELECT_BATTERY_PROFILE:
if (!ARMING_FLAG(ARMED)) {
if (sbufReadU8Safe(&tmp_u8, src))

10
src/main/fc/fc_msp_box.c
View file

@ -29,6 +29,7 @@
#include "fc/config.h"
#include "fc/fc_msp_box.h"
#include "fc/runtime_config.h"
#include "flight/mixer.h"
#include "io/osd.h"
@ -79,6 +80,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXOSDALT2, "OSD ALT 2", 43 },
{ BOXOSDALT3, "OSD ALT 3", 44 },
{ BOXNAVCRUISE, "NAV CRUISE", 45 },
{ BOXBRAKING, "MC BRAKING", 46 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -202,6 +204,13 @@ void initActiveBoxIds(void)
}
}
}
#ifdef USE_MR_BRAKING_MODE
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
activeBoxIds[activeBoxIdCount++] = BOXBRAKING;
}
#endif
#endif
if (STATE(FIXED_WING)) {
@ -319,6 +328,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3);
CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING);
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) {

1
src/main/fc/rc_modes.h
View file

@ -60,6 +60,7 @@ typedef enum {
BOXOSDALT2 = 33,
BOXOSDALT3 = 34,
BOXNAVCRUISE = 35,
BOXBRAKING = 36,
CHECKBOX_ITEM_COUNT
} boxId_e;

5
src/main/fc/runtime_config.h
View file

@ -100,7 +100,10 @@ typedef enum {
COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED= (1 << 9),
PWM_DRIVER_AVAILABLE = (1 << 10),
HELICOPTER = (1 << 11)
HELICOPTER = (1 << 11),
NAV_CRUISE_BRAKING = (1 << 12),
NAV_CRUISE_BRAKING_BOOST = (1 << 13),
NAV_CRUISE_BRAKING_LOCKED = (1 << 14),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

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

@ -1307,7 +1307,46 @@ groups:
field: mc.auto_disarm_delay
min: 100
max: 10000
- name: nav_mc_braking_speed_threshold
field: mc.braking_speed_threshold
condition: USE_MR_BRAKING_MODE
min: 0
max: 1000
- name: nav_mc_braking_disengage_speed
field: mc.braking_disengage_speed
condition: USE_MR_BRAKING_MODE
min: 0
max: 1000
- name: nav_mc_braking_timeout
field: mc.braking_timeout
condition: USE_MR_BRAKING_MODE
min: 100
max: 5000
- name: nav_mc_braking_boost_factor
field: mc.braking_boost_factor
condition: USE_MR_BRAKING_MODE
min: 0
max: 200
- name: nav_mc_braking_boost_timeout
field: mc.braking_boost_timeout
condition: USE_MR_BRAKING_MODE
min: 0
max: 5000
- name: nav_mc_braking_boost_speed_threshold
field: mc.braking_boost_speed_threshold
condition: USE_MR_BRAKING_MODE
min: 100
max: 1000
- name: nav_mc_braking_boost_disengage_speed
field: mc.braking_boost_disengage_speed
condition: USE_MR_BRAKING_MODE
min: 0
max: 1000
- name: nav_mc_braking_bank_angle
field: mc.braking_bank_angle
condition: USE_MR_BRAKING_MODE
min: 15
max: 60
- name: nav_fw_cruise_thr
field: fw.cruise_throttle
min: 1000

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -29,6 +29,8 @@
#define MSP2_INAV_SET_RATE_PROFILE 0x2008
#define MSP2_INAV_AIR_SPEED 0x2009
#define MSP2_INAV_OUTPUT_MAPPING 0x200A
#define MSP2_INAV_MC_BRAKING 0x200B
#define MSP2_INAV_SET_MC_BRAKING 0x200C
#define MSP2_INAV_MIXER 0x2010
#define MSP2_INAV_SET_MIXER 0x2011

106
src/main/navigation/navigation.c
View file

@ -116,9 +116,17 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// MC-specific
.mc = {
.max_bank_angle = 30, // 30 deg
.max_bank_angle = 30, // 30 deg
.hover_throttle = 1500,
.auto_disarm_delay = 2000,
.braking_speed_threshold = 100, // Braking can become active above 1m/s
.braking_disengage_speed = 75, // Stop when speed goes below 0.75m/s
.braking_timeout = 2000, // Timeout barking after 2s
.braking_boost_factor = 100, // A 100% boost by default
.braking_boost_timeout = 750, // Timout boost after 750ms
.braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s
.braking_boost_disengage_speed = 100, // Disable boost at 1m/s
.braking_bank_angle = 40, // Max braking angle
},
// Fixed wing
@ -2161,8 +2169,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos)
*-----------------------------------------------------------*/
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
{
// XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
// XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
posControl.desiredState.pos.x = pos->x;
posControl.desiredState.pos.y = pos->y;
}
@ -2333,6 +2341,90 @@ static void resetPositionController(void)
}
}
static void processBrakingMode(const bool isAdjusting)
{
#ifdef USE_MR_BRAKING_MODE
static uint32_t brakingModeDisengageAt = 0;
static uint32_t brakingBoostModeDisengageAt = 0;
const bool brakingEntryAllowed =
IS_RC_MODE_ACTIVE(BOXBRAKING) &&
!STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!isAdjusting &&
navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE &&
navConfig()->mc.braking_speed_threshold > 0 &&
(
NAV_Status.state == MW_NAV_STATE_NONE ||
NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT
);
/*
* Case one, when we order to brake (sticks to the center) and we are moving above threshold
* Speed is above 1m/s and sticks are centered
* Extra condition: BRAKING flight mode has to be enabled
*/
if (brakingEntryAllowed) {
/*
* Set currnt position and target position
* Enabling NAV_CRUISE_BRAKING locks other routines from setting position!
*/
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
ENABLE_STATE(NAV_CRUISE_BRAKING);
//Set forced BRAKING disengage moment
brakingModeDisengageAt = millis() + navConfig()->mc.braking_timeout;
//If speed above threshold, start boost mode as well
if (posControl.actualState.velXY > navConfig()->mc.braking_boost_speed_threshold) {
ENABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
brakingBoostModeDisengageAt = millis() + navConfig()->mc.braking_boost_timeout;
}
}
// We can enter braking only after user started to move the sticks
if (STATE(NAV_CRUISE_BRAKING_LOCKED) && isAdjusting) {
DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
}
/*
* Case when speed dropped, disengage BREAKING_BOOST
*/
if (
STATE(NAV_CRUISE_BRAKING_BOOST) && (
posControl.actualState.velXY <= navConfig()->mc.braking_boost_disengage_speed ||
brakingBoostModeDisengageAt < millis()
)) {
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
}
/*
* Case when we were braking but copter finally stopped or we started to move the sticks
*/
if (STATE(NAV_CRUISE_BRAKING) && (
posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed || //We stopped
isAdjusting || //Moved the sticks
brakingModeDisengageAt < millis() //Braking is done to timed disengage
)) {
DISABLE_STATE(NAV_CRUISE_BRAKING);
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
/*
* When braking is done, store current position as desired one
* We do not want to go back to the place where braking has started
*/
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
}
#endif
}
static bool adjustPositionFromRCInput(void)
{
bool retValue;
@ -2341,7 +2433,13 @@ static bool adjustPositionFromRCInput(void)
retValue = adjustFixedWingPositionFromRCInput();
}
else {
retValue = adjustMulticopterPositionFromRCInput();
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
processBrakingMode(rcPitchAdjustment || rcRollAdjustment);
retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
}
return retValue;

14
src/main/navigation/navigation.h
View file

@ -148,9 +148,17 @@ typedef struct navConfig_s {
} general;
struct {
uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t hover_throttle; // multicopter hover throttle
uint16_t auto_disarm_delay; // multicopter safety delay for landing detector
uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t hover_throttle; // multicopter hover throttle
uint16_t auto_disarm_delay; // multicopter safety delay for landing detector
uint16_t braking_speed_threshold; // above this speed braking routine might kick in
uint16_t braking_disengage_speed; // below this speed braking will be disengaged
uint16_t braking_timeout; // Timeout for braking mode
uint8_t braking_boost_factor; // Acceleration boost multiplier at max speed
uint16_t braking_boost_timeout; // Timeout for boost mode
uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase
} mc;
struct {

58
src/main/navigation/navigation_multicopter.c
View file

@ -276,11 +276,8 @@ void resetMulticopterPositionController(void)
}
}
bool adjustMulticopterPositionFromRCInput(void)
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment)
{
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
if (rcPitchAdjustment || rcRollAdjustment) {
// If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID
if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) {
@ -382,15 +379,22 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
if (velErrorMagnitude > 0.1f) {
accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
}
else {
} else {
accelLimitX = maxAccelLimit / 1.414213f;
accelLimitY = accelLimitX;
}
// Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate
// This will assure that we wont't saturate out LEVEL and RATE PID controller
const float maxAccelChange = US2S(deltaMicros) * 1700.0f;
float maxAccelChange = US2S(deltaMicros) * 1700.0f;
//When braking, raise jerk limit even if we are not boosting acceleration
#ifdef USE_MR_BRAKING_MODE
if (STATE(NAV_CRUISE_BRAKING)) {
maxAccelChange = maxAccelChange * 2;
}
#endif
const float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX);
const float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX);
const float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY);
@ -401,16 +405,47 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
// Apply PID with output limiting and I-term anti-windup
// Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
// Thus we don't need to do anything else with calculated acceleration
const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle);
uint8_t accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ;
#ifdef USE_MR_BRAKING_MODE
//Boost required accelerations
if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0)
{
const float rawBoostFactor = (100.0f + (float)navConfig()->mc.braking_boost_factor) / 100.0f;
//Scale boost factor according to speed
const float boostFactor = constrainf(
scaleRangef(
posControl.actualState.velXY,
navConfig()->mc.braking_boost_speed_threshold,
navConfig()->general.max_manual_speed,
0,
rawBoostFactor
),
0,
rawBoostFactor
);
//Boost required acceleration for harder braking
newAccelX = newAccelX * boostFactor;
newAccelY = newAccelY * boostFactor;
maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.braking_bank_angle);
accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ * 2;
}
#endif
// Save last acceleration target
lastAccelTargetX = newAccelX;
lastAccelTargetY = newAccelY;
// Apply LPF to jerk limited acceleration target
const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, accCutoffFrequency, US2S(deltaMicros));
const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, accCutoffFrequency, US2S(deltaMicros));
// Rotate acceleration target into forward-right frame (aircraft)
const float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;
@ -420,7 +455,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
const int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle);
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
}

2
src/main/navigation/navigation_private.h
View file

@ -392,7 +392,7 @@ void resetMulticopterHeadingController(void);
bool adjustMulticopterAltitudeFromRCInput(void);
bool adjustMulticopterHeadingFromRCInput(void);
bool adjustMulticopterPositionFromRCInput(void);
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);

2
src/main/target/OMNIBUS/target.h
View file

@ -163,3 +163,5 @@
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#undef USE_MR_BRAKING_MODE

1
src/main/target/common.h
View file

@ -56,6 +56,7 @@
#define USE_TELEMETRY_FRSKY
#define USE_GYRO_BIQUAD_RC_FIR2
#define USE_MR_BRAKING_MODE
#if defined(STM_FAST_TARGET)
#define SCHEDULER_DELAY_LIMIT 10