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

Fix center when no valid headtracker data is available

Browse source
This commit is contained in:
Marcelo Bezerra 2024-06-22 14:03:06 +02:00
parent 055642ac67
commit 7ffb0bc500
1 changed files with 8 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/io/gimbal_serial.c
View file

@ -60,6 +60,7 @@ static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
static gimbalSerialHtrkState_t headTrackerState = {
.payloadSize = 0,
.attitude = {},
.state = WAITING_HDR1,
};
#endif
@ -189,7 +190,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
gimbalHtkAttitudePkt_t attitude = {
.sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
.mode = GIMBAL_MODE_DEFAULT
.mode = GIMBAL_MODE_DEFAULT,
.pan = 0,
.tilt = 0,
.roll = 0
};
const gimbalConfig_t *cfg = gimbalConfig();
@ -238,9 +242,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
} else {
attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
}
} else {