diff options
Diffstat (limited to 'src/ms5607.cpp')
| -rw-r--r-- | src/ms5607.cpp | 208 |
1 files changed, 208 insertions, 0 deletions
diff --git a/src/ms5607.cpp b/src/ms5607.cpp new file mode 100644 index 0000000..c093189 --- /dev/null +++ b/src/ms5607.cpp @@ -0,0 +1,208 @@ +#include "ms5607.hpp" + +static const int32_t altitude_table[] = { +#include "altitude-pa.h" +}; + +#define ALT_SCALE (1 << ALT_SHIFT) +#define ALT_MASK (ALT_SCALE - 1) + +void MS5607::initialize() { + sample_state = NOT_SAMPLING; + + alarm_pool_init_default(); + + ms5607_cmd cmd; + cmd.data = RESET_COMMAND; + + ms5607_write_cmd(&cmd); + + sleep_ms(500); + + cmd.data = 0; + cmd.fields.PROM = true; + cmd.fields.PROM2 = true; + + cmd.fields.ADDR_OSR = PROM_CALIBRATION_COEFFICENT_1_ADDR; + + for (uint8_t prom_addr = PROM_CALIBRATION_COEFFICENT_1_ADDR; prom_addr <= PROM_CALIBRATION_COEFFICENT_6_ADDR; prom_addr++) { + sleep_ms(100); + cmd.fields.ADDR_OSR = prom_addr; + ms5607_write_cmd(&cmd); + i2c_read_blocking(i2c, addr, buffer, 2, false); + + prom[prom_addr - 1] = static_cast<uint16_t>((((uint16_t) buffer[0]) << 8) | ((uint16_t) buffer[1])); + } +} + +void MS5607::ms5607_write_cmd(ms5607_cmd* cmd) { + i2c_write_blocking(i2c, addr, (uint8_t *) cmd, 1, false); +} + +void MS5607::ms5607_start_sample() { + if (sample_state == NOT_SAMPLING) { + sample_state = PRESSURE_CONVERT; + ms5607_sample(); + } +} + +#if (USE_FREERTOS == 1) +void MS5607::ms5607_sample_handler(void* pvParameters) { + /* xMaxExpectedBlockTime is set to be a little longer than the maximum + expected time between events. */ + const TickType_t xInterruptFrequency = pdMS_TO_TICKS( 1000 / (MS5607_SAMPLE_RATE_HZ * 2) ); + const TickType_t xMaxExpectedBlockTime = xInterruptFrequency + pdMS_TO_TICKS( 1 ); + uint32_t ulEventsToProcess; + while (1) { + /* Wait to receive a notification sent directly to this task from the + interrupt service routine. */ + ulEventsToProcess = ulTaskNotifyTake( pdTRUE, xMaxExpectedBlockTime ); + if( ulEventsToProcess != 0 ) { + /* To get here at least one event must have occurred. Loop here + until all the pending events have been processed */ + while( ulEventsToProcess > 0 ) { + MS5607* alt = (MS5607 *) pvParameters; + taskENTER_CRITICAL(); + alt->ms5607_sample(); + taskEXIT_CRITICAL(); + ulEventsToProcess--; + } + } + } + +} + +void MS5607::update_ms5607_task(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MS5607_SAMPLE_RATE_HZ); + + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + MS5607* alt = (MS5607 *) pvParameters; + taskENTER_CRITICAL(); + alt->ms5607_start_sample(); + taskEXIT_CRITICAL(); + } +} +#endif + +int64_t MS5607::ms5607_sample_callback(alarm_id_t id, void* user_data) { +#if ( USE_FREERTOS == 1 ) + BaseType_t xHigherPriorityTaskWoken; + xHigherPriorityTaskWoken = pdFALSE; + MS5607* alt = (MS5607 *) user_data; + // Defer ISR handling to separate handler within FreeRTOS context + vTaskNotifyGiveFromISR(alt->sample_handler_task_handle, &xHigherPriorityTaskWoken ); + portYIELD_FROM_ISR( xHigherPriorityTaskWoken ); +#else + MS5607* alt = (MS5607 *) user_data; + alt->ms5607_sample(); +#endif + return 0; +} + +void MS5607::ms5607_sample() { + ms5607_cmd cmd = {.data = 0}; + + switch (sample_state) { + case NOT_SAMPLING: + break; + case PRESSURE_CONVERT: { + cmd.fields.CONVERT = 1; + cmd.fields.TYPE = TYPE_UNCOMPENSATED_PRESSURE; + cmd.fields.ADDR_OSR = OSR_CONVERT_256; + + ms5607_write_cmd(&cmd); + + add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true); + + sample_state = TEMPERATURE_CONVERT; + break; + }; + case TEMPERATURE_CONVERT: { + cmd.data = ADC_READ_COMMAND; + ms5607_write_cmd(&cmd); + i2c_read_blocking(i2c, addr, buffer, 3, false); + uncompensated_pressure = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]); + + cmd.fields.CONVERT = 1; + cmd.fields.TYPE = TYPE_UNCOMPENSATED_TEMPERATURE; + cmd.fields.ADDR_OSR = OSR_CONVERT_256; + + ms5607_write_cmd(&cmd); + + add_alarm_in_us(OSR_256_CONVERSION_TIME_US, MS5607::ms5607_sample_callback, (void *) this, true); + + sample_state = COMPENSATE; + break; + }; + case COMPENSATE: { + cmd.data = ADC_READ_COMMAND; + ms5607_write_cmd(&cmd); + i2c_read_blocking(i2c, addr, buffer, 3, false); + uncompensated_temperature = (((uint32_t) buffer[0]) << 16) | (((uint32_t) buffer[1]) << 8) | ((uint32_t) buffer[2]); + ms5607_compensate(); + altitude = pressure_to_altitude(pressure); + + + sample_state = NOT_SAMPLING; + if (threshold_callback != NULL) { + if (positive_crossing) { + if (altitude >= threshold_altitude) { + add_alarm_in_ms(1, threshold_callback, NULL, true); + } + } else { + if (altitude <= threshold_altitude) { + add_alarm_in_ms(1, threshold_callback, NULL, true); + } + } + } + break; + }; + }; + +} + +void MS5607::ms5607_compensate() { + int32_t dT = uncompensated_temperature - (((uint32_t) prom[4]) << 8); + temperature = 2000 + ( ( ( (int64_t) dT) * ( (int64_t) prom[5]) ) >> 23); + int64_t OFF = ( ( (int64_t) prom[1]) << 17) + ( ( ((int64_t) prom[3]) * ( (int64_t) dT)) >> 6); + int64_t SENS = ( ( (int64_t) prom[0]) << 16) + (( ( (int64_t) prom[2]) * ((int64_t) dT)) >> 7); + pressure = (int32_t) ((((((int64_t) uncompensated_pressure) * SENS) >> 21) - OFF) >> 15); + +} + +int32_t MS5607::pressure_to_altitude(int32_t pressure) { + uint16_t o; + int16_t part; + int32_t low, high; + + if (pressure < 0) { + pressure = 0; + } + + if (pressure > 120000) { + pressure = 120000; + } + + o = (uint16_t) (pressure >> ALT_SHIFT); + part = pressure & ALT_MASK; + + low = (int32_t) altitude_table[o] * (ALT_SCALE - part); + high = (int32_t) altitude_table[o + 1] * part; + return ((low + high + (ALT_SCALE >> 1)) >> ALT_SHIFT); +} + + +void MS5607::set_threshold_altitude(int32_t threshold_altitude, alarm_callback_t callback) { + this->threshold_altitude = threshold_altitude; + + positive_crossing = (threshold_altitude > altitude); + + threshold_callback = callback; +} + +void MS5607::clear_threshold_altitude() { + this->threshold_callback = NULL; +} |