mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 06:15:14 +03:00
initial build
This commit is contained in:
breadoven
parent
fd59159f71
commit
505b58f9f0
9 changed files with 216 additions and 176 deletions
|
|
@ -2652,6 +2652,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
|
|||
|
||||
---
|
||||
|
||||
### nav_cruise_yaw_rate
|
||||
|
||||
Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 20 | 0 | 60 |
|
||||
|
||||
---
|
||||
|
||||
### nav_disarm_on_landing
|
||||
|
||||
If set to ON, INAV disarms the FC after landing
|
||||
|
|
@ -2742,16 +2752,6 @@ Cruise throttle in GPS assisted modes, this includes RTH. Should be set high eno
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_cruise_yaw_rate
|
||||
|
||||
Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 20 | 0 | 60 |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_dive_angle
|
||||
|
||||
Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit
|
||||
|
|
|
|||
|
|
@ -227,6 +227,8 @@ void initActiveBoxIds(void)
|
|||
if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) {
|
||||
ADD_ACTIVE_BOX(BOXNAVRTH);
|
||||
ADD_ACTIVE_BOX(BOXNAVWP);
|
||||
ADD_ACTIVE_BOX(BOXNAVCRUISE);
|
||||
ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
|
||||
ADD_ACTIVE_BOX(BOXHOMERESET);
|
||||
ADD_ACTIVE_BOX(BOXGCSNAV);
|
||||
ADD_ACTIVE_BOX(BOXPLANWPMISSION);
|
||||
|
|
@ -236,8 +238,6 @@ void initActiveBoxIds(void)
|
|||
}
|
||||
|
||||
if (STATE(AIRPLANE)) {
|
||||
ADD_ACTIVE_BOX(BOXNAVCRUISE);
|
||||
ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
|
||||
ADD_ACTIVE_BOX(BOXSOARING);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2571,6 +2571,12 @@ groups:
|
|||
default_value: ON
|
||||
field: general.flags.mission_planner_reset
|
||||
type: bool
|
||||
- name: nav_cruise_yaw_rate
|
||||
description: "Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]"
|
||||
default_value: 20
|
||||
field: general.cruise_yaw_rate
|
||||
min: 0
|
||||
max: 60
|
||||
- name: nav_mc_bank_angle
|
||||
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
|
||||
default_value: 30
|
||||
|
|
@ -2806,12 +2812,6 @@ groups:
|
|||
field: fw.launch_abort_deadband
|
||||
min: 2
|
||||
max: 250
|
||||
- name: nav_fw_cruise_yaw_rate
|
||||
description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]"
|
||||
default_value: 20
|
||||
field: fw.cruise_yaw_rate
|
||||
min: 0
|
||||
max: 60
|
||||
- name: nav_fw_allow_manual_thr_increase
|
||||
description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing"
|
||||
default_value: OFF
|
||||
|
|
|
|||
|
|
@ -900,17 +900,14 @@ float pidHeadingHold(float dT)
|
|||
{
|
||||
float headingHoldRate;
|
||||
|
||||
/* Convert absolute error into relative to current heading */
|
||||
int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
|
||||
|
||||
/*
|
||||
* Convert absolute error into relative to current heading
|
||||
*/
|
||||
if (error <= -180) {
|
||||
error += 360;
|
||||
}
|
||||
|
||||
if (error >= +180) {
|
||||
/* Convert absolute error into relative to current heading */
|
||||
if (error > 180) {
|
||||
error -= 360;
|
||||
} else if (error < -180) {
|
||||
error += 360;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -159,6 +159,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -213,7 +214,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
|
||||
|
||||
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
|
|
@ -428,7 +428,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -487,7 +487,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -1061,10 +1061,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 1);
|
||||
// Switch to IDLE if we do not have an healty position. Try the next iteration.
|
||||
if (checkForPositionSensorTimeout()) {
|
||||
|
|
@ -1073,7 +1069,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
|
||||
resetPositionController();
|
||||
|
||||
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
|
||||
if (STATE(AIRPLANE)) {
|
||||
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
|
||||
} else { // Multicopter
|
||||
posControl.cruise.course = posControl.actualState.yaw;
|
||||
posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
|
||||
posControl.desiredState.pos = posControl.actualState.abs.pos;
|
||||
}
|
||||
posControl.cruise.previousCourse = posControl.cruise.course;
|
||||
posControl.cruise.lastCourseAdjustmentTime = 0;
|
||||
|
||||
|
|
@ -1094,7 +1096,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
DEBUG_SET(DEBUG_CRUISE, 0, 2);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch
|
||||
return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
|
||||
}
|
||||
|
||||
|
|
@ -1102,7 +1104,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
if (posControl.flags.isAdjustingHeading) {
|
||||
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
|
||||
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
|
||||
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
|
||||
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate));
|
||||
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
|
||||
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
|
||||
|
|
@ -1135,8 +1137,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);
|
||||
|
|
@ -2469,27 +2469,27 @@ void checkSafeHomeState(bool shouldBeEnabled)
|
|||
posControl.flags.rthTrackbackActive ||
|
||||
(!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance);
|
||||
|
||||
if (safehomeNotApplicable) {
|
||||
shouldBeEnabled = false;
|
||||
} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
|
||||
// if safehomes are only used with failsafe and we're trying to enable safehome
|
||||
// then enable the safehome only with failsafe
|
||||
shouldBeEnabled = posControl.flags.forcedRTHActivated;
|
||||
}
|
||||
if (safehomeNotApplicable) {
|
||||
shouldBeEnabled = false;
|
||||
} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
|
||||
// if safehomes are only used with failsafe and we're trying to enable safehome
|
||||
// then enable the safehome only with failsafe
|
||||
shouldBeEnabled = posControl.flags.forcedRTHActivated;
|
||||
}
|
||||
// no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
|
||||
if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
|
||||
return;
|
||||
}
|
||||
if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
|
||||
return;
|
||||
}
|
||||
if (shouldBeEnabled) {
|
||||
// set home to safehome
|
||||
// set home to safehome
|
||||
setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = true;
|
||||
} else {
|
||||
// set home to original arming point
|
||||
safehome_applied = true;
|
||||
} else {
|
||||
// set home to original arming point
|
||||
setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = false;
|
||||
}
|
||||
// if we've changed the home position, update the distance and direction
|
||||
safehome_applied = false;
|
||||
}
|
||||
// if we've changed the home position, update the distance and direction
|
||||
updateHomePosition();
|
||||
}
|
||||
|
||||
|
|
@ -2506,8 +2506,8 @@ bool findNearestSafeHome(void)
|
|||
gpsLocation_t shLLH;
|
||||
shLLH.alt = 0;
|
||||
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
|
||||
if (!safeHomeConfig(i)->enabled)
|
||||
continue;
|
||||
if (!safeHomeConfig(i)->enabled)
|
||||
continue;
|
||||
|
||||
shLLH.lat = safeHomeConfig(i)->lat;
|
||||
shLLH.lon = safeHomeConfig(i)->lon;
|
||||
|
|
@ -2523,7 +2523,7 @@ bool findNearestSafeHome(void)
|
|||
}
|
||||
}
|
||||
if (safehome_index >= 0) {
|
||||
safehome_distance = nearest_safehome_distance;
|
||||
safehome_distance = nearest_safehome_distance;
|
||||
} else {
|
||||
safehome_distance = 0;
|
||||
}
|
||||
|
|
@ -3441,33 +3441,34 @@ bool isNavHoldPositionActive(void)
|
|||
navigationIsExecutingAnEmergencyLanding();
|
||||
}
|
||||
|
||||
float getActiveWaypointSpeed(void)
|
||||
float getActiveSpeed(void)
|
||||
{
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
// In manual control mode use different cap for speed
|
||||
/* Currently only applicable for multicopter */
|
||||
|
||||
// Speed limit for modes where speed manually controlled
|
||||
if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return navConfig()->general.max_manual_speed;
|
||||
}
|
||||
else {
|
||||
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
||||
float wpSpecificSpeed = 0.0f;
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
|
||||
else
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
|
||||
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||
|
||||
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = wpSpecificSpeed;
|
||||
} else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = navConfig()->general.max_auto_speed;
|
||||
}
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
|
||||
float wpSpecificSpeed = 0.0f;
|
||||
if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
|
||||
else
|
||||
wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
|
||||
|
||||
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = wpSpecificSpeed;
|
||||
} else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
|
||||
waypointSpeed = navConfig()->general.max_auto_speed;
|
||||
}
|
||||
}
|
||||
|
||||
return waypointSpeed;
|
||||
}
|
||||
|
||||
return waypointSpeed;
|
||||
}
|
||||
|
||||
bool isWaypointNavTrackingActive(void)
|
||||
|
|
@ -3486,29 +3487,21 @@ static void processNavigationRCAdjustments(void)
|
|||
{
|
||||
/* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
}
|
||||
|
||||
if (navStateFlags & NAV_RC_POS) {
|
||||
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE);
|
||||
if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
|
||||
resetMulticopterBrakingMode();
|
||||
}
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
posControl.flags.isAdjustingPosition = false;
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
}
|
||||
posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
|
||||
posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
|
||||
posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3864,17 +3857,16 @@ int8_t navigationGetHeadingControlState(void)
|
|||
}
|
||||
|
||||
// For multirotors it depends on navigation system mode
|
||||
// Course hold requires Auto Control to update heading hold target whilst RC adjustment active
|
||||
if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
else {
|
||||
return NAV_HEADING_CONTROL_AUTO;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return NAV_HEADING_CONTROL_NONE;
|
||||
|
||||
return NAV_HEADING_CONTROL_AUTO;
|
||||
}
|
||||
|
||||
return NAV_HEADING_CONTROL_NONE;
|
||||
}
|
||||
|
||||
bool navigationTerrainFollowingEnabled(void)
|
||||
|
|
@ -4411,3 +4403,8 @@ bool isAdjustingHeading(void) {
|
|||
int32_t getCruiseHeadingAdjustment(void) {
|
||||
return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -268,6 +268,7 @@ typedef struct navConfig_s {
|
|||
uint8_t land_detect_sensitivity; // Sensitivity of landing detector
|
||||
uint16_t auto_disarm_delay; // safety time delay for landing detector
|
||||
uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
} general;
|
||||
|
||||
struct {
|
||||
|
|
@ -315,7 +316,6 @@ typedef struct navConfig_s {
|
|||
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
|
||||
bool launch_manual_throttle; // Allows launch with manual throttle control
|
||||
uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
bool allow_manual_thr_increase;
|
||||
bool useFwNavYawControl;
|
||||
uint8_t yawControlDeadband;
|
||||
|
|
|
|||
|
|
@ -71,7 +71,6 @@ static bool isRollAdjustmentValid = false;
|
|||
static bool isYawAdjustmentValid = false;
|
||||
static float throttleSpeedAdjustment = 0;
|
||||
static bool isAutoThrottleManuallyIncreased = false;
|
||||
static int32_t navHeadingError;
|
||||
static float navCrossTrackError;
|
||||
static int8_t loiterDirYaw = 1;
|
||||
bool needToCalculateCircularLoiter;
|
||||
|
|
@ -440,7 +439,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
* Calculate NAV heading error
|
||||
* Units are centidegrees
|
||||
*/
|
||||
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
|
||||
int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
|
||||
|
||||
// Forced turn direction
|
||||
// If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
|
||||
|
|
@ -845,11 +844,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
}
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return navHeadingError;
|
||||
}
|
||||
|
||||
float navigationGetCrossTrackError(void)
|
||||
{
|
||||
return navCrossTrackError;
|
||||
|
|
|
|||
|
|
@ -222,7 +222,7 @@ void resetMulticopterAltitudeController(void)
|
|||
pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
|
||||
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
|
||||
const float maxSpeed = getActiveWaypointSpeed();
|
||||
const float maxSpeed = getActiveSpeed();
|
||||
nav_speed_up = maxSpeed;
|
||||
nav_accel_z = maxSpeed;
|
||||
nav_speed_down = navConfig()->general.max_auto_climb_rate;
|
||||
|
|
@ -289,14 +289,15 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
bool adjustMulticopterHeadingFromRCInput(void)
|
||||
{
|
||||
if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
|
||||
// Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home
|
||||
posControl.desiredState.yaw = posControl.actualState.yaw;
|
||||
// Heading during Cruise Hold mode set by Nav function so no adjustment required here
|
||||
if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
posControl.desiredState.yaw = posControl.actualState.yaw;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -406,8 +407,44 @@ void resetMulticopterPositionController(void)
|
|||
}
|
||||
}
|
||||
|
||||
static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment)
|
||||
{
|
||||
static timeMs_t lastUpdateTimeMs;
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
const timeMs_t updateDeltaTimeMs = currentTimeMs - lastUpdateTimeMs;
|
||||
lastUpdateTimeMs = currentTimeMs;
|
||||
|
||||
const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband);
|
||||
|
||||
if (rcVelX > posControl.cruise.multicopterSpeed) {
|
||||
posControl.cruise.multicopterSpeed = rcVelX;
|
||||
} else if (rcVelX < 0 && updateDeltaTimeMs < 100) {
|
||||
posControl.cruise.multicopterSpeed += MS2S(updateDeltaTimeMs) * rcVelX / 2.0f;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
posControl.cruise.multicopterSpeed = constrainf(posControl.cruise.multicopterSpeed, 10.0f, navConfig()->general.max_manual_speed);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void setMulticopterStopPosition(void)
|
||||
{
|
||||
fpVector3_t stopPosition;
|
||||
calculateMulticopterInitialHoldPosition(&stopPosition);
|
||||
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
|
||||
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment)
|
||||
{
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
if (rcPitchAdjustment) {
|
||||
return adjustMulticopterCruiseSpeed(rcPitchAdjustment);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Process braking mode
|
||||
processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment);
|
||||
|
||||
|
|
@ -429,16 +466,12 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
|
|||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
else if (posControl.flags.isAdjustingPosition) {
|
||||
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
fpVector3_t stopPosition;
|
||||
calculateMulticopterInitialHoldPosition(&stopPosition);
|
||||
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
|
||||
return false;
|
||||
setMulticopterStopPosition();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static float getVelocityHeadingAttenuationFactor(void)
|
||||
|
|
@ -467,15 +500,28 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
|
|||
|
||||
static void updatePositionVelocityController_MC(const float maxSpeed)
|
||||
{
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
// Position held at cruise speeds below 0.5 m/s, otherwise desired neu velocities set directly from cruise speed
|
||||
if (posControl.cruise.multicopterSpeed >= 50) {
|
||||
// Rotate multicopter x velocity from body frame to earth frame
|
||||
posControl.desiredState.vel.x = posControl.cruise.multicopterSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
|
||||
posControl.desiredState.vel.y = posControl.cruise.multicopterSpeed * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
|
||||
|
||||
return;
|
||||
} else if (posControl.flags.isAdjustingPosition) {
|
||||
setMulticopterStopPosition();
|
||||
}
|
||||
}
|
||||
|
||||
const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
// Calculate target velocity
|
||||
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
|
||||
float neuVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP;
|
||||
|
||||
// Scale velocity to respect max_speed
|
||||
float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY);
|
||||
float neuVelTotal = calc_length_pythagorean_2D(neuVelX, neuVelY);
|
||||
|
||||
/*
|
||||
* We override computed speed with max speed in following cases:
|
||||
|
|
@ -485,26 +531,23 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
|
|||
if (
|
||||
((navGetCurrentStateFlags() & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) &&
|
||||
!isNavHoldPositionActive() &&
|
||||
newVelTotal < maxSpeed &&
|
||||
neuVelTotal < maxSpeed &&
|
||||
!navConfig()->mc.slowDownForTurning
|
||||
) || newVelTotal > maxSpeed
|
||||
) || neuVelTotal > maxSpeed
|
||||
) {
|
||||
newVelX = maxSpeed * (newVelX / newVelTotal);
|
||||
newVelY = maxSpeed * (newVelY / newVelTotal);
|
||||
newVelTotal = maxSpeed;
|
||||
neuVelX = maxSpeed * (neuVelX / neuVelTotal);
|
||||
neuVelY = maxSpeed * (neuVelY / neuVelTotal);
|
||||
neuVelTotal = maxSpeed;
|
||||
}
|
||||
|
||||
posControl.pids.pos[X].output_constrained = newVelX;
|
||||
posControl.pids.pos[Y].output_constrained = newVelY;
|
||||
posControl.pids.pos[X].output_constrained = neuVelX;
|
||||
posControl.pids.pos[Y].output_constrained = neuVelY;
|
||||
|
||||
// Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
|
||||
const float velHeadFactor = getVelocityHeadingAttenuationFactor();
|
||||
const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
|
||||
posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
|
||||
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed);
|
||||
posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor;
|
||||
}
|
||||
|
||||
static float computeNormalizedVelocity(const float value, const float maxValue)
|
||||
|
|
@ -664,49 +707,53 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
|
||||
static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
bool bypassPositionController;
|
||||
|
||||
// We should passthrough rcCommand is adjusting position in GPS_ATTI mode
|
||||
bypassPositionController = (navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition;
|
||||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
// and pilots input would be passed thru to PID controller
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
||||
if (!bypassPositionController) {
|
||||
// Update position controller
|
||||
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
|
||||
// Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
|
||||
const float maxSpeed = getActiveWaypointSpeed();
|
||||
updatePositionVelocityController_MC(maxSpeed);
|
||||
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first start or glitch), reset altitude controller
|
||||
resetMulticopterPositionController();
|
||||
}
|
||||
}
|
||||
|
||||
// Indicate that information is no longer usable
|
||||
posControl.flags.horizontalPositionDataConsumed = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (posControl.flags.estPosStatus < EST_USABLE) {
|
||||
/* No position data, disable automatic adjustment, rcCommand passthrough */
|
||||
posControl.rcAdjustment[PITCH] = 0;
|
||||
posControl.rcAdjustment[ROLL] = 0;
|
||||
bypassPositionController = true;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (!bypassPositionController) {
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
// Passthrough rcCommand if adjusting position in GPS_ATTI mode except when Course Hold active
|
||||
bool bypassPositionController = !FLIGHT_MODE(NAV_COURSE_HOLD_MODE) &&
|
||||
navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI &&
|
||||
posControl.flags.isAdjustingPosition;
|
||||
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
// Indicate that information is no longer usable
|
||||
posControl.flags.horizontalPositionDataConsumed = true;
|
||||
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
||||
if (bypassPositionController) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If we have new position data - update velocity and acceleration controllers
|
||||
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
|
||||
// Get max speed for current NAV mode
|
||||
float maxSpeed = getActiveSpeed();
|
||||
updatePositionVelocityController_MC(maxSpeed);
|
||||
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
|
||||
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first start or glitch), reset position controller
|
||||
resetMulticopterPositionController();
|
||||
}
|
||||
} else if (bypassPositionController) {
|
||||
return;
|
||||
}
|
||||
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
}
|
||||
|
||||
bool isMulticopterFlying(void)
|
||||
|
|
@ -933,6 +980,10 @@ void resetMulticopterHeadingController(void)
|
|||
|
||||
static void applyMulticopterHeadingController(void)
|
||||
{
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // heading set by Nav during Course Hold so disable yaw stick input
|
||||
rcCommand[YAW] = 0;
|
||||
}
|
||||
|
||||
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -325,6 +325,7 @@ typedef struct {
|
|||
int32_t course;
|
||||
int32_t previousCourse;
|
||||
timeMs_t lastCourseAdjustmentTime;
|
||||
float multicopterSpeed;
|
||||
} navCruise_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -451,7 +452,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
|
||||
bool isNavHoldPositionActive(void);
|
||||
bool isLastMissionWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
float getActiveSpeed(void);
|
||||
bool isWaypointNavTrackingActive(void);
|
||||
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue