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

Merge pull request #5430 from iNavFlight/dzikuvx-step-1-state-fixed-wing-untanglement

Browse source 
Disable flight modes for ROVERS and BOATS
This commit is contained in:
Paweł Spychalski 2020-02-22 17:14:30 +01:00 • committed by GitHub
commit ec76d499dc
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 31 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/fc/fc_init.c
View file

@ -287,6 +287,9 @@ void init(void)
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_REVERSIBLE_MOTORS);
}
if (!STATE(ALTITUDE_CONTROL)) {
featureClear(FEATURE_AIRMODE);
}
// Initialize motor and servo outpus
if (pwmMotorAndServoInit()) {

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

@ -164,13 +164,13 @@ void initActiveBoxIds(void)
activeBoxIdCount = 0;
activeBoxIds[activeBoxIdCount++] = BOXARM;
if (sensors(SENSOR_ACC)) {
if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
}
if (!feature(FEATURE_AIRMODE)) {
if (!feature(FEATURE_AIRMODE) && STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
}
@ -181,35 +181,39 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
}
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
if (STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
}
//Camstab mode is enabled always
activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB;
#ifdef USE_GPS
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (STATE(AIRPLANE) && feature(FEATURE_GPS)))) {
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
}
const bool navReadyQuads = !STATE(FIXED_WING_LEGACY) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyPlanes = STATE(FIXED_WING_LEGACY) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS);
const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
if (STATE(FIXED_WING_LEGACY)) {
if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) {
if (!STATE(ROVER) && !STATE(BOAT)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
}
if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN;
}
}
if (navReadyQuads || navReadyPlanes) {
if (navReadyMultirotor || navReadyOther) {
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
if (feature(FEATURE_GPS)) {
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
}
}
@ -223,8 +227,11 @@ void initActiveBoxIds(void)
#endif
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
activeBoxIds[activeBoxIdCount++] = BOXMANUAL;
}
if (STATE(AIRPLANE)) {
if (!feature(FEATURE_FW_LAUNCH)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
}

2
src/main/fc/rc_controls.c
View file

@ -183,7 +183,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
emergencyArmingUpdate(armingSwitchIsActive);
if (STATE(FIXED_WING_LEGACY) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
tryArm();

4
src/main/fc/rc_modes.c
View file

@ -104,9 +104,9 @@ static void processAirmodeMultirotor(void) {
void processAirmode(void) {
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE)) {
processAirmodeAirplane();
} else {
} else if (STATE(MULTIROTOR)) {
processAirmodeMultirotor();
}

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

@ -127,7 +127,7 @@ typedef enum {
MULTIROTOR = (1 << 18),
ROVER = (1 << 19),
BOAT = (1 << 20),
ALTITUDE_CONTROL = (1 << 21),
ALTITUDE_CONTROL = (1 << 21), //It means it can fly
MOVE_FORWARD_ONLY = (1 << 22),
} stateFlags_t;

2
src/main/flight/hil.c
View file

@ -57,7 +57,7 @@ void hilUpdateControlState(void)
{
// FIXME: There must be a cleaner way to to this
// If HIL active, store PID outout into hilState and disable motor control
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(FIXED_WING_LEGACY)) {
if (FLIGHT_MODE(MANUAL_MODE) || !STATE(AIRPLANE)) {
hilToSIM.pidCommand[ROLL] = rcCommand[ROLL];
hilToSIM.pidCommand[PITCH] = rcCommand[PITCH];
hilToSIM.pidCommand[YAW] = rcCommand[YAW];

8
src/main/flight/pid.c
View file

@ -530,7 +530,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
@ -857,7 +857,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
targetRates.x = 0.0f;
targetRates.y = 0.0f;
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE)) {
if (calculateCosTiltAngle() >= 0.173648f) {
// Ideal banked turn follow the equations:
// forward_vel^2 / radius = Gravity * tan(roll_angle)
@ -901,7 +901,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
// Replace YAW on quads - add it in on airplanes
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE)) {
pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
}
else {
@ -1014,7 +1014,7 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType;
}
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE;
}