Skip to content

Add blackbox target heading + missing Nav auto enabled flight modes #9249

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Aug 25, 2023
Merged

Add blackbox target heading + missing Nav auto enabled flight modes #9249

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
90d8a0e
add target heading
breadoven Aug 19, 2023
1524cc5
Add missing flight modes
breadoven Aug 22, 2023
41b493a
Update blackbox.c
breadoven Aug 22, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 23 additions & 9 deletions src/main/blackbox/blackbox.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -289,23 +289,23 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},

{"gyroRaw", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
{"gyroRaw", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
{"gyroRaw", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},

{"gyroPeakRoll", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
{"gyroPeakRoll", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
{"gyroPeakRoll", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},

{"gyroPeakPitch", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
{"gyroPeakPitch", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
{"gyroPeakPitch", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},

{"gyroPeakYaw", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
{"gyroPeakYaw", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
{"gyroPeakYaw", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},


{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
Expand Down Expand Up @@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
Expand Down Expand Up @@ -517,7 +518,7 @@ typedef struct blackboxMainState_s {
int16_t navTargetVel[XYZ_AXIS_COUNT];
int32_t navTargetPos[XYZ_AXIS_COUNT];
int16_t navHeading;
int16_t navTargetHeading;
uint16_t navTargetHeading;
int16_t navSurface;
} blackboxMainState_t;

Expand Down Expand Up @@ -740,7 +741,7 @@ static void blackboxBuildConditionCache(void)
{
blackboxConditionCache = 0;
for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {

const uint64_t position = ((uint64_t)1) << cond;

if (testBlackboxConditionUncached(cond)) {
Expand Down Expand Up @@ -891,7 +892,7 @@ static void writeIntraframe(void)
}

blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);

if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) {
blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT);
}
Expand Down Expand Up @@ -970,6 +971,7 @@ static void writeIntraframe(void)
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
}

blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
blackboxWriteSignedVB(blackboxCurrent->navSurface);
}

Expand Down Expand Up @@ -1226,6 +1228,7 @@ static void writeInterframe(void)
blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
}

blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
}

Expand Down Expand Up @@ -1298,6 +1301,16 @@ static void writeSlowFrame(void)
static void loadSlowState(blackboxSlowState_t *slow)
{
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
// Also log Nav auto selected flight modes rather than just those selected by boxmode
if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
slow->flightModeFlags |= (1 << BOXANGLE);
}
if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
}
if (navigationRequiresTurnAssistance()) {
slow->flightModeFlags |= (1 << BOXTURNASSIST);
}
slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase();
slow->rxSignalReceived = rxIsReceivingSignal();
Expand Down Expand Up @@ -1328,7 +1341,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
bool valid_temp;
int16_t newTemp = 0;
valid_temp = getIMUTemperature(&newTemp);
if (valid_temp)
if (valid_temp)
slow->imuTemperature = newTemp;
else
slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
Expand Down Expand Up @@ -1661,6 +1674,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
}
blackboxCurrent->navTargetHeading = navDesiredHeading;
blackboxCurrent->navSurface = navActualSurface;
}

Expand Down
6 changes: 2 additions & 4 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -886,8 +886,6 @@ static uint8_t getHeadingHoldState(void)
}
else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
return HEADING_HOLD_ENABLED;
} else {
return HEADING_HOLD_UPDATE_HEADING;
}

return HEADING_HOLD_UPDATE_HEADING;
Expand Down Expand Up @@ -1124,10 +1122,10 @@ void FAST_CODE pidController(float dT)
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
}
}
}

if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Assuming this is an error ?

if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
Expand Down
11 changes: 6 additions & 5 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -235,10 +235,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
int16_t navCurrentState;
int16_t navActualVelocity[3];
int16_t navDesiredVelocity[3];
int16_t navActualHeading;
int16_t navDesiredHeading;
int32_t navTargetPosition[3];
int32_t navLatestActualPosition[3];
int16_t navActualHeading;
uint16_t navDesiredHeading;
int16_t navActualSurface;
uint16_t navFlags;
uint16_t navEPH;
Expand Down Expand Up @@ -3564,6 +3564,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);

navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
}

/*-----------------------------------------------------------
Expand Down Expand Up @@ -3834,9 +3836,8 @@ bool navigationRequiresTurnAssistance(void)
// For airplanes turn assistant is always required when controlling position
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
}
else {
return false;
}

return false;
}

/**
Expand Down
1 change: 1 addition & 0 deletions src/main/navigation/navigation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -638,6 +638,7 @@ extern int16_t navActualVelocity[3];
extern int16_t navDesiredVelocity[3];
extern int32_t navTargetPosition[3];
extern int32_t navLatestActualPosition[3];
extern uint16_t navDesiredHeading;
extern int16_t navActualSurface;
extern uint16_t navFlags;
extern uint16_t navEPH;
Expand Down