diff options
Diffstat (limited to 'src/mmc5983ma.cpp')
| -rw-r--r-- | src/mmc5983ma.cpp | 158 |
1 files changed, 158 insertions, 0 deletions
diff --git a/src/mmc5983ma.cpp b/src/mmc5983ma.cpp new file mode 100644 index 0000000..45ef94a --- /dev/null +++ b/src/mmc5983ma.cpp @@ -0,0 +1,158 @@ +#include "mmc5983ma.hpp" + +int16_t MMC5983MA::sat_sub(int16_t a, int16_t b) { + int32_t result = (int32_t) a - (int32_t) b; + if (result < INT16_MIN) { + result = INT16_MIN; + } + if (result > INT16_MAX) { + result = INT16_MAX; + } + return (int16_t) result; +}; + +void MMC5983MA::initialize() { + // Restart device prior to configuration + buffer[0] = R_MMC5983MA_INTERNAL_CTL1; + internal_ctl1.fields.RESTART = true; + buffer[1] = internal_ctl1.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + internal_ctl1.data = 0; + + sleep_ms(100); + + calibrate(); + + //Configure decimation filter bandwidth for 200 Hz + buffer[0] = R_MMC5983MA_INTERNAL_CTL1; + internal_ctl1.fields.BANDWIDTH = B_MMC5983MA_BANDWIDTH_200HZ; + buffer[1] = internal_ctl1.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + + //Configure and enable continuous measurement mode for 200 Hz + buffer[0] = R_MMC5983MA_INTERNAL_CTL2; + internal_ctl2.fields.CONTINUOUS_MODE_ENABLE = true; + internal_ctl2.fields.CONTINUOUS_MODE_FREQ = B_MMC5983_CONTINUOUS_MODE_FREQ_200HZ; + internal_ctl2.fields.PERIODIC_SET_ENABLE = false; + internal_ctl2.fields.PERIODIC_SET_RATE = B_MMC5983_PERIODIC_SET_RATE_MEAS_TIMES_1000; + buffer[1] = internal_ctl2.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + printf("MMC5983MA Initialized!\n"); +} + +void MMC5983MA::sample() { + buffer[0] = R_MMC5983MA_XOUT0; + i2c_write_blocking(i2c, addr, buffer, 1, true); + i2c_read_blocking(i2c, addr, buffer, 6, false); + + raw_x = (int16_t) ((((uint16_t) buffer[0] << 8) | buffer[1]) - 32768); + raw_y = (int16_t) ((((uint16_t) buffer[2] << 8) | buffer[3]) - 32768); + raw_z = (int16_t) ((((uint16_t) buffer[4] << 8) | buffer[5]) - 32768); +} + +void MMC5983MA::apply_offset() { + ax = sat_sub(raw_x, offset_x); + ay = sat_sub(raw_y, offset_y); + az = sat_sub(raw_z, offset_z); +} + +void MMC5983MA::calibrate() { + sleep_ms(100); + + printf("MMC5983MA Calibration | Set command"); + // Take measurement after SET command completes + buffer[0] = R_MMC5983MA_INTERNAL_CTL0; + internal_ctl0.fields.SET_CMD = true; + buffer[1] = internal_ctl0.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + internal_ctl0.data = 0; + + sleep_ms(100); + printf(" | Measurement Request"); + + buffer[0] = R_MMC5983MA_INTERNAL_CTL0; + internal_ctl0.fields.TAKE_MAG_MEAS = true; + buffer[1] = internal_ctl0.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + internal_ctl0.data = 0; + printf(" | Request sent, awaiting completion...\n"); + + size_t counter = 0; + while (!dev_status.fields.MEAS_M_DONE) { + sleep_ms(1); + buffer[0] = R_MMC5983MA_DEV_STATUS; + i2c_write_blocking(i2c, addr, buffer, 1, true); + i2c_read_blocking(i2c, addr, buffer, 1, false); + dev_status.data = buffer[0]; + printf("W | "); + counter++; + if (counter == 20) { + printf("\n"); + counter = 0; + } + } + printf("\n\nMeasurement taken!"); + + sample(); + printf(" | Sample taken!"); + + int16_t set_ax = raw_x, set_ay = raw_y, set_az = raw_z; + + // Take measurement after RESET command completes + buffer[0] = R_MMC5983MA_INTERNAL_CTL0; + internal_ctl0.fields.RESET_CMD = true; + buffer[1] = internal_ctl0.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + internal_ctl0.data = 0; + + sleep_ms(100); + + buffer[0] = R_MMC5983MA_INTERNAL_CTL0; + internal_ctl0.fields.TAKE_MAG_MEAS = true; + buffer[1] = internal_ctl0.data; + i2c_write_blocking(i2c, addr, buffer, 2, false); + internal_ctl0.data = 0; + + while (!dev_status.fields.MEAS_M_DONE) { + sleep_ms(1); + buffer[0] = R_MMC5983MA_DEV_STATUS; + i2c_write_blocking(i2c, addr, buffer, 1, true); + i2c_read_blocking(i2c, addr, buffer, 1, false); + dev_status.data = buffer[0]; + } + + sample(); + + int16_t reset_ax = raw_x, reset_ay = raw_y, reset_az = raw_z; + + sleep_ms(100); + + // Average the two measurements taken and assign them as offsets + offset_x = (int16_t) (((int32_t) set_ax + (int32_t) reset_ax) / 2); + offset_y = (int16_t) (((int32_t) set_ay + (int32_t) reset_ay) / 2); + offset_z = (int16_t) (((int32_t) set_az + (int32_t) reset_az) / 2); + +#if ( DEBUG == 1 ) + printf("MMC5983MA: Calibrated!\n\toffset_x: %" PRIi16 "\n\toffset_y: %" PRIi16 "\n\toffset_z: %" PRIi16 "\n", offset_x, offset_y, offset_z); +#endif +} + +#if (USE_FREERTOS == 1) +void MMC5983MA::update_mmc5983ma_task(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MMC5983_SAMPLE_RATE_HZ); + + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + taskENTER_CRITICAL(); + MMC5983MA* mag = (MMC5983MA *) pvParameters; + mag->sample(); + mag->apply_offset(); + taskEXIT_CRITICAL(); + if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) { + xLastWakeTime = xTaskGetTickCount(); + } + } +} +#endif |