Skip to content

Commit 44caafd

Browse files
DzikuVxDzikuVx
authored
Merge pull request #9249 from breadoven/abo_blackbox_add_target_heading
Add blackbox target heading + missing Nav auto enabled flight modes
2 parents fb16203 + 41b493a commit 44caafd

File tree

4 files changed

+32
-18
lines changed

4 files changed

+32
-18
lines changed

‎src/main/blackbox/blackbox.c‎

Lines changed: 23 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -289,23 +289,23 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
289289
{"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
290290
{"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
291291
{"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
292-
292+
293293
{"gyroRaw", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
294294
{"gyroRaw", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
295295
{"gyroRaw", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
296-
296+
297297
{"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},
298298
{"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},
299299
{"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},
300-
300+
301301
{"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},
302302
{"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},
303303
{"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},
304-
304+
305305
{"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},
306306
{"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},
307307
{"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},
308-
308+
309309

310310
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
311311
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
@@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
366366
{"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
367367
{"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
368368
{"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
369+
{"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
369370
{"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
370371
{"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
371372
{"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
@@ -517,7 +518,7 @@ typedef struct blackboxMainState_s {
517518
int16_t navTargetVel[XYZ_AXIS_COUNT];
518519
int32_t navTargetPos[XYZ_AXIS_COUNT];
519520
int16_t navHeading;
520-
int16_t navTargetHeading;
521+
uint16_t navTargetHeading;
521522
int16_t navSurface;
522523
} blackboxMainState_t;
523524

@@ -740,7 +741,7 @@ static void blackboxBuildConditionCache(void)
740741
{
741742
blackboxConditionCache = 0;
742743
for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
743-
744+
744745
const uint64_t position = ((uint64_t)1) << cond;
745746

746747
if (testBlackboxConditionUncached(cond)) {
@@ -891,7 +892,7 @@ static void writeIntraframe(void)
891892
}
892893

893894
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
894-
895+
895896
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) {
896897
blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT);
897898
}
@@ -970,6 +971,7 @@ static void writeIntraframe(void)
970971
blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
971972
}
972973

974+
blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
973975
blackboxWriteSignedVB(blackboxCurrent->navSurface);
974976
}
975977

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

1231+
blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
12291232
blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
12301233
}
12311234

@@ -1298,6 +1301,16 @@ static void writeSlowFrame(void)
12981301
static void loadSlowState(blackboxSlowState_t *slow)
12991302
{
13001303
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
1304+
// Also log Nav auto selected flight modes rather than just those selected by boxmode
1305+
if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
1306+
slow->flightModeFlags |= (1 << BOXANGLE);
1307+
}
1308+
if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
1309+
slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
1310+
}
1311+
if (navigationRequiresTurnAssistance()) {
1312+
slow->flightModeFlags |= (1 << BOXTURNASSIST);
1313+
}
13011314
slow->stateFlags = stateFlags;
13021315
slow->failsafePhase = failsafePhase();
13031316
slow->rxSignalReceived = rxIsReceivingSignal();
@@ -1328,7 +1341,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
13281341
bool valid_temp;
13291342
int16_t newTemp = 0;
13301343
valid_temp = getIMUTemperature(&newTemp);
1331-
if (valid_temp)
1344+
if (valid_temp)
13321345
slow->imuTemperature = newTemp;
13331346
else
13341347
slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
@@ -1661,6 +1674,7 @@ static void loadMainState(timeUs_t currentTimeUs)
16611674
blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
16621675
blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
16631676
}
1677+
blackboxCurrent->navTargetHeading = navDesiredHeading;
16641678
blackboxCurrent->navSurface = navActualSurface;
16651679
}
16661680

‎src/main/flight/pid.c‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -886,8 +886,6 @@ static uint8_t getHeadingHoldState(void)
886886
}
887887
else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
888888
return HEADING_HOLD_ENABLED;
889-
} else {
890-
return HEADING_HOLD_UPDATE_HEADING;
891889
}
892890

893891
return HEADING_HOLD_UPDATE_HEADING;
@@ -1124,10 +1122,10 @@ void FAST_CODE pidController(float dT)
11241122
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
11251123
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
11261124
levelingEnabled = true;
1127-
}
1125+
}
11281126
}
11291127

1130-
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
1128+
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
11311129
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
11321130
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
11331131
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);

‎src/main/navigation/navigation.c‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -235,10 +235,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
235235
int16_t navCurrentState;
236236
int16_t navActualVelocity[3];
237237
int16_t navDesiredVelocity[3];
238-
int16_t navActualHeading;
239-
int16_t navDesiredHeading;
240238
int32_t navTargetPosition[3];
241239
int32_t navLatestActualPosition[3];
240+
int16_t navActualHeading;
241+
uint16_t navDesiredHeading;
242242
int16_t navActualSurface;
243243
uint16_t navFlags;
244244
uint16_t navEPH;
@@ -3564,6 +3564,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
35643564
navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
35653565
navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
35663566
navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
3567+
3568+
navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
35673569
}
35683570

35693571
/*-----------------------------------------------------------
@@ -3834,9 +3836,8 @@ bool navigationRequiresTurnAssistance(void)
38343836
// For airplanes turn assistant is always required when controlling position
38353837
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
38363838
}
3837-
else {
3838-
return false;
3839-
}
3839+
3840+
return false;
38403841
}
38413842

38423843
/**

‎src/main/navigation/navigation.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -644,6 +644,7 @@ extern int16_t navActualVelocity[3];
644644
extern int16_t navDesiredVelocity[3];
645645
extern int32_t navTargetPosition[3];
646646
extern int32_t navLatestActualPosition[3];
647+
extern uint16_t navDesiredHeading;
647648
extern int16_t navActualSurface;
648649
extern uint16_t navFlags;
649650
extern uint16_t navEPH;

0 commit comments

Comments
 (0)