Skip to content
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

Use barometer data for altitude hold #97

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
+21 −10
Draft

Use barometer data for altitude hold #97

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 13 additions & 10 deletions components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -305,18 +305,21 @@ void sensorsMpu6050Hmc5883lMs5611WaitDataReady(void)

void processBarometerMeasurements(const uint8_t *buffer)
{
//TODO: replace it to MS5611
DEBUG_PRINTW("processBarometerMeasurements NEED TODO");
// static uint32_t rawPressure = 0;
// static int16_t rawTemp = 0;
static uint32_t rawPressure = 0;
static int16_t rawTemp = 0;

// Check if there is a new pressure update

// Check if there is a new temp update
// Check if there is a new pressure update
if (buffer[0] & 0x02) {
rawPressure = ((uint32_t) buffer[3] << 16) | ((uint32_t) buffer[2] << 8) | buffer[1];
}
// Check if there is a new temp update
if (buffer[0] & 0x01) {
rawTemp = ((int16_t) buffer[5] << 8) | buffer[4];
}

// sensorData.baro.pressure = (float) rawPressure / LPS25H_LSB_PER_MBAR;
// sensorData.baro.temperature = LPS25H_TEMP_OFFSET + ((float) rawTemp / LPS25H_LSB_PER_CELSIUS);
// sensorData.baro.asl = lps25hPressureToAltitude(&sensorData.baro.pressure);
sensorData.baro.pressure = (float) rawPressure / MS5611_LSB_PER_MBAR;
sensorData.baro.temperature = MS5611_TEMP_OFFSET + ((float) rawTemp / MS5611_LSB_PER_CELSIUS);
sensorData.baro.asl = ms5611PressureToAltitude(&sensorData.baro.pressure);
}

void processMagnetometerMeasurements(const uint8_t *buffer)
Expand Down
8 changes: 8 additions & 0 deletions components/core/crazyflie/modules/src/position_estimator_altitude.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ struct selfState_s {
float vAccDeadband; // Vertical acceleration deadband
float velZAlpha; // Blending factor to avoid vertical speed to accumulate error
float estimatedVZ;
float baroAlpha; // Blending factor for barometer data
};

static struct selfState_s state = {
Expand All @@ -55,6 +56,7 @@ static struct selfState_s state = {
.vAccDeadband = 0.04f,
.velZAlpha = 0.995f,
.estimatedVZ = 0.0f,
.baroAlpha = 0.9f,
};

static void positionEstimateInternal(state_t* estimate, const sensorData_t* sensorData, const tofMeasurement_t* tofMeasurement, float dt, uint32_t tick, struct selfState_s* state);
Expand Down Expand Up @@ -103,6 +105,9 @@ static void positionEstimateInternal(state_t* estimate, const sensorData_t* sens
state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt);
}

// Incorporate barometer data for altitude estimation
state->estimatedZ = (state->baroAlpha * state->estimatedZ) + ((1.0f - state->baroAlpha) * sensorData->baro.asl);

estimate->position.x = 0.0f;
estimate->position.y = 0.0f;
estimate->position.z = state->estimatedZ;
Expand All @@ -114,6 +119,8 @@ static void positionEstimateInternal(state_t* estimate, const sensorData_t* sens
static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfState_s* state) {
state->velocityZ += deadband(accWZ, state->vAccDeadband) * dt * G;
state->velocityZ *= state->velZAlpha;
// Update vertical velocity using barometer data
state->velocityZ = (state->baroAlpha * state->velocityZ) + ((1.0f - state->baroAlpha) * state->estimatedVZ);
}

LOG_GROUP_START(posEstAlt)
Expand All @@ -128,4 +135,5 @@ PARAM_ADD(PARAM_FLOAT, estAlphaZr, &state.estAlphaZrange)
PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor)
PARAM_ADD(PARAM_FLOAT, velZAlpha, &state.velZAlpha)
PARAM_ADD(PARAM_FLOAT, vAccDeadband, &state.vAccDeadband)
PARAM_ADD(PARAM_FLOAT, baroAlpha, &state.baroAlpha)
PARAM_GROUP_STOP(posEstAlt)