mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 17:55:28 +03:00
FW launch: Wiggle to wake idle throttle
- Added options for detection for different type/size planes - Changed trigger to only use the yaw axis gyro data. - Time limit for individual wiggle to prevent accidental triggers - Time limit for double wiggle, again to prevent accidental triggers.
This commit is contained in:
Darren Lines
parent
369fafdabc
commit
92d51edc5e
5 changed files with 127 additions and 63 deletions
|
|
@ -2974,11 +2974,11 @@ Launch abort stick deadband in [r/c points], applied after r/c deadband and expo
|
|||
|
||||
### nav_fw_launch_accel
|
||||
|
||||
Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s
|
||||
Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 1863 | 1000 | 20000 |
|
||||
| 1863 | 1500 | 20000 |
|
||||
|
||||
---
|
||||
|
||||
|
|
@ -3032,16 +3032,6 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_launch_jerk_wake_idle
|
||||
|
||||
Trigger the idle throttle by jerking the plane
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| OFF | OFF | ON |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_launch_manual_throttle
|
||||
|
||||
Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised).
|
||||
|
|
@ -3132,6 +3122,16 @@ Forward velocity threshold for swing-launch detection [cm/s]
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_launch_wiggle_to_wake_idle
|
||||
|
||||
Trigger the idle throttle by wiggling the plane. 0 = disabled. 1 and 2 signify 1 or 2 yaw wiggles to activate. 1 wiggle has a higher detection point, for airplanes without a tail. 2 wiggles has a lower detection point, but requires the repeated action. This is intended for larger models and airplanes with tails.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0 | 0 | 2 |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_loiter_radius
|
||||
|
||||
PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]
|
||||
|
|
|
|||
|
|
@ -2774,10 +2774,10 @@ groups:
|
|||
min: 100
|
||||
max: 10000
|
||||
- name: nav_fw_launch_accel
|
||||
description: "Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s"
|
||||
description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s"
|
||||
default_value: 1863
|
||||
field: fw.launch_accel_thresh
|
||||
min: 1000
|
||||
min: 1500
|
||||
max: 20000
|
||||
- name: nav_fw_launch_max_angle
|
||||
description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]"
|
||||
|
|
@ -2797,11 +2797,13 @@ groups:
|
|||
field: fw.launch_idle_motor_timer
|
||||
min: 0
|
||||
max: 60000
|
||||
- name: nav_fw_launch_jerk_wake_idle
|
||||
description: "Trigger the idle throttle by jerking the plane"
|
||||
field: fw.launch_jerk_wake_idle
|
||||
type: bool
|
||||
default_value: OFF
|
||||
- name: nav_fw_launch_wiggle_to_wake_idle
|
||||
description: "Trigger the idle throttle by wiggling the plane. 0 = disabled. 1 and 2 signify 1 or 2 yaw wiggles to activate. 1 wiggle has a higher detection point, for airplanes without a tail. 2 wiggles has a lower detection point, but requires the repeated action. This is intended for larger models and airplanes with tails."
|
||||
field: fw.launch_wiggle_wake_idle
|
||||
type: uint8_t
|
||||
default_value: 0
|
||||
min: 0
|
||||
max: 2
|
||||
- name: nav_fw_launch_motor_delay
|
||||
description: "Delay between detected launch and launch sequence start and throttling up (ms)"
|
||||
default_value: 500
|
||||
|
|
|
|||
|
|
@ -196,7 +196,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
|
||||
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_jerk_wake_idle = SETTING_NAV_FW_LAUNCH_JERK_WAKE_IDLE_DEFAULT, // bool
|
||||
.launch_wiggle_wake_idle = SETTING_NAV_FW_LAUNCH_WIGGLE_TO_WAKE_IDLE_DEFAULT, // uint8_t
|
||||
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
|
||||
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
|
||||
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
|
||||
|
|
@ -204,13 +204,13 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
|
||||
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
|
||||
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
|
||||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.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,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled
|
||||
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
|
||||
}
|
||||
);
|
||||
|
|
|
|||
|
|
@ -145,9 +145,9 @@ typedef enum {
|
|||
} navRTHClimbFirst_e;
|
||||
|
||||
typedef enum { // keep aligned with fixedWingLaunchState_t
|
||||
FW_LAUNCH_DETECTED = 4,
|
||||
FW_LAUNCH_ABORTED = 9,
|
||||
FW_LAUNCH_FLYING = 10,
|
||||
FW_LAUNCH_DETECTED = 5,
|
||||
FW_LAUNCH_ABORTED = 10,
|
||||
FW_LAUNCH_FLYING = 11,
|
||||
} navFwLaunchStatus_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -294,7 +294,7 @@ typedef struct navConfig_s {
|
|||
uint16_t launch_time_thresh; // Time threshold for launch detection (ms)
|
||||
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
|
||||
uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms)
|
||||
bool launch_jerk_wake_idle; // Activate the idle throttle by jerking the plane
|
||||
uint8_t launch_wiggle_wake_idle; // Activate the idle throttle by wiggling the plane. 0 = disabled, 1 or 2 specify the number of wiggles.
|
||||
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
|
||||
uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
|
||||
uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@
|
|||
#include "io/gps.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
|
||||
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
|
||||
|
|
@ -79,20 +80,22 @@ typedef enum {
|
|||
|
||||
typedef enum { // if changed update navFwLaunchStatus_e
|
||||
FW_LAUNCH_STATE_WAIT_THROTTLE = 0,
|
||||
FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
|
||||
FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
|
||||
FW_LAUNCH_STATE_MOTOR_IDLE,
|
||||
FW_LAUNCH_STATE_WAIT_DETECTION,
|
||||
FW_LAUNCH_STATE_DETECTED, // 4
|
||||
FW_LAUNCH_STATE_DETECTED, // FW_LAUNCH_DETECTED = 5
|
||||
FW_LAUNCH_STATE_MOTOR_DELAY,
|
||||
FW_LAUNCH_STATE_MOTOR_SPINUP,
|
||||
FW_LAUNCH_STATE_IN_PROGRESS,
|
||||
FW_LAUNCH_STATE_FINISH,
|
||||
FW_LAUNCH_STATE_ABORTED, // 9
|
||||
FW_LAUNCH_STATE_FLYING, // 10
|
||||
FW_LAUNCH_STATE_ABORTED, // FW_LAUNCH_ABORTED = 10
|
||||
FW_LAUNCH_STATE_FLYING, // FW_LAUNCH_FLYING = 11
|
||||
FW_LAUNCH_STATE_COUNT
|
||||
} fixedWingLaunchState_t;
|
||||
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs);
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs);
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
|
||||
|
|
@ -124,12 +127,22 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
|
|||
[FW_LAUNCH_STATE_WAIT_THROTTLE] = {
|
||||
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
|
||||
.onEvent = {
|
||||
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
|
||||
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
|
||||
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION
|
||||
},
|
||||
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
|
||||
},
|
||||
|
||||
[FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT] = {
|
||||
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
|
||||
.onEvent = {
|
||||
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE,
|
||||
[FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
|
||||
[FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE,
|
||||
},
|
||||
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
|
||||
},
|
||||
|
||||
[FW_LAUNCH_STATE_IDLE_MOTOR_DELAY] = {
|
||||
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
|
||||
.onEvent = {
|
||||
|
|
@ -253,6 +266,47 @@ static inline bool isThrottleLow(void)
|
|||
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
|
||||
}
|
||||
|
||||
static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
|
||||
static timeMs_t wiggleTime = 0;
|
||||
static timeMs_t wigglesTime = 0;
|
||||
static int8_t wiggleStageOne = 0;
|
||||
static uint8_t wiggleCount = 0;
|
||||
const bool isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
|
||||
const uint8_t wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40;
|
||||
int8_t wiggleDirection = 0;
|
||||
int16_t yawRate = (int16_t)(gyroRateDps(YAW) * (4 / 16.4));
|
||||
|
||||
// Check to see if yaw rate has exceeded 50 dps. If so proceed to the next stage or continue to idle
|
||||
if ((yawRate < -wiggleStrength || yawRate > wiggleStrength) && isAircraftWithinLaunchAngle) {
|
||||
wiggleDirection = (yawRate > 0) ? 1 : -1;
|
||||
|
||||
if (wiggleStageOne == 0) {
|
||||
wiggleStageOne = wiggleDirection;
|
||||
wigglesTime = US2MS(currentTimeUs);
|
||||
} else if (wiggleStageOne != wiggleDirection) {
|
||||
wiggleStageOne = 0;
|
||||
wiggleCount++;
|
||||
|
||||
if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
wiggleTime = US2MS(currentTimeUs);
|
||||
}
|
||||
|
||||
// If time between wiggle stages is > 100ms, or the time between two wiggles is > 1s. Reset the wiggle
|
||||
if (
|
||||
((wiggleStageOne != 0) && (US2MS(currentTimeUs) > (wiggleTime + 100))) ||
|
||||
((wiggleCount != 0) && (US2MS(currentTimeUs) > (wigglesTime + 500)))
|
||||
) {
|
||||
wiggleStageOne = 0;
|
||||
wiggleCount = 0;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static inline bool isLaunchMaxAltitudeReached(void)
|
||||
{
|
||||
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
|
||||
|
|
@ -310,26 +364,34 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
|
|||
return FW_LAUNCH_EVENT_NONE;
|
||||
}
|
||||
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs) {
|
||||
if (isThrottleLow()) {
|
||||
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
|
||||
}
|
||||
|
||||
if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) {
|
||||
return FW_LAUNCH_EVENT_GOTO_DETECTION;
|
||||
}
|
||||
|
||||
applyThrottleIdleLogic(true);
|
||||
|
||||
if (hasIdleWakeWiggleSucceeded(currentTimeUs)) {
|
||||
idleMotorAboutToStart = false;
|
||||
return FW_LAUNCH_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
return FW_LAUNCH_EVENT_NONE;
|
||||
}
|
||||
|
||||
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
|
||||
{
|
||||
bool isIdleJerkActivationTriggered = false;
|
||||
|
||||
if (isThrottleLow()) {
|
||||
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
|
||||
}
|
||||
|
||||
applyThrottleIdleLogic(true);
|
||||
|
||||
// Check to see if the plane has been jerked. If so, wake the idle throttle now
|
||||
if (navConfig()->fw.launch_jerk_wake_idle) {
|
||||
const float preIdleSwingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
|
||||
const bool isPreIdleSwingLaunched = (preIdleSwingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
|
||||
const bool isPreIdleForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
|
||||
|
||||
isIdleJerkActivationTriggered = (isPreIdleSwingLaunched || isPreIdleForwardAccelerationHigh);
|
||||
}
|
||||
|
||||
if ((currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) || isIdleJerkActivationTriggered) {
|
||||
if ((currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) || (navConfig()->fw.launch_wiggle_wake_idle > 0 && hasIdleWakeWiggleSucceeded(currentTimeUs))) {
|
||||
idleMotorAboutToStart = false;
|
||||
return FW_LAUNCH_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue