diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f342298ee7..1c83dd3f3f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -6,7 +6,6 @@ * @author Thomas Gubler * @author Julian Oes * - * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: @@ -104,6 +103,8 @@ static int stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; +static void do_gyro_calibration(void); +static void do_mag_calibration(void); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); /* pthread loops */ @@ -212,16 +213,175 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -void do_gyro_calibration(void) +void cal_bsort(int16_t a[], int n) { + int i,j,t; + for(i=0;ia[j+1]) { + t=a[j]; + a[j]=a[j+1]; + a[j+1]=t; + } + } + } +} + +void do_mag_calibration(void) +{ + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + /* 30 seconds */ + const uint64_t calibration_interval_us = 5 * 1000000; + unsigned int calibration_counter = 0; + + const int peak_samples = 2000; + /* Get rid of 10% */ + const int outlier_margin = (peak_samples) / 10; + + int16_t *mag_maxima[3]; + mag_maxima[0] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + mag_maxima[1] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + mag_maxima[2] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + + int16_t *mag_minima[3]; + mag_minima[0] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + mag_minima[1] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + mag_minima[2] = (int16_t*)calloc(peak_samples, sizeof(uint16_t)); + + /* initialize data table */ + for (int i = 0; i < peak_samples; i++) { + mag_maxima[0][i] = INT16_MIN; + mag_maxima[1][i] = INT16_MIN; + mag_maxima[2][i] = INT16_MIN; + + mag_minima[0][i] = INT16_MAX; + mag_minima[1][i] = INT16_MAX; + mag_minima[2][i] = INT16_MAX; + } + + uint64_t calibration_start = hrt_absolute_time(); + while ((hrt_absolute_time() - calibration_start) < calibration_interval_us + && calibration_counter < peak_samples) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + if (poll(fds, 1, 1000)) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + /* get min/max values */ + + /* iterate through full list */ + for (int i = 0; i < peak_samples; i++) { + /* x minimum */ + if (raw.magnetometer_raw[0] < mag_minima[0][i]) + mag_minima[0][i] = raw.magnetometer_raw[0]; + /* y minimum */ + if (raw.magnetometer_raw[1] < mag_minima[1][i]) + mag_minima[1][i] = raw.magnetometer_raw[1]; + /* z minimum */ + if (raw.magnetometer_raw[2] < mag_minima[2][i]) + mag_minima[2][i] = raw.magnetometer_raw[2]; + + /* x maximum */ + if (raw.magnetometer_raw[0] > mag_maxima[0][i]) + mag_maxima[0][i] = raw.magnetometer_raw[0]; + /* y maximum */ + if (raw.magnetometer_raw[1] > mag_maxima[1][i]) + mag_maxima[1][i] = raw.magnetometer_raw[1]; + /* z maximum */ + if (raw.magnetometer_raw[2] > mag_maxima[2][i]) + mag_maxima[2][i] = raw.magnetometer_raw[2]; + } + + calibration_counter++; + } else { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; + } + } + /* sort values */ + cal_bsort(mag_minima[0], peak_samples); + cal_bsort(mag_minima[1], peak_samples); + cal_bsort(mag_minima[2], peak_samples); + + cal_bsort(mag_maxima[0], peak_samples); + cal_bsort(mag_maxima[1], peak_samples); + cal_bsort(mag_maxima[2], peak_samples); + + float min_avg[3] = { 0.0f, 0.0f, 0.0f }; + float max_avg[3] = { 0.0f, 0.0f, 0.0f }; + + /* take average of center value group */ + for (int i = 0; i < (peak_samples - outlier_margin); i++) { + min_avg[0] += mag_minima[0][i+outlier_margin]; + min_avg[1] += mag_minima[1][i+outlier_margin]; + min_avg[2] += mag_minima[2][i+outlier_margin]; + + max_avg[0] += mag_maxima[0][i]; + max_avg[1] += mag_maxima[1][i]; + max_avg[2] += mag_maxima[2][i]; + + if (i > (peak_samples - outlier_margin)-15) { + printf("mag min: %d\t%d\t%d\tmag max: %d\t%d\t%d\n", + mag_minima[0][i+outlier_margin], + mag_minima[1][i+outlier_margin], + mag_minima[2][i+outlier_margin], + mag_maxima[0][i], + mag_maxima[1][i], + mag_maxima[2][i]); + usleep(10000); + } + } + + min_avg[0] /= (peak_samples - outlier_margin); + min_avg[1] /= (peak_samples - outlier_margin); + min_avg[2] /= (peak_samples - outlier_margin); + + max_avg[0] /= (peak_samples - outlier_margin); + max_avg[1] /= (peak_samples - outlier_margin); + max_avg[2] /= (peak_samples - outlier_margin); + + printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]); + + int16_t mag_offset[3]; + mag_offset[0] = (max_avg[0] - min_avg[0])/2; + mag_offset[1] = (max_avg[1] - min_avg[1])/2; + mag_offset[2] = (max_avg[2] - min_avg[2])/2; + + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET] = mag_offset[0]; + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET] = mag_offset[1]; + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET] = mag_offset[2]; + + free(mag_maxima[0]); + free(mag_maxima[1]); + free(mag_maxima[2]); + + free(mag_minima[0]); + free(mag_minima[1]); + free(mag_minima[2]); + + char offset_output[50]; + sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]); + mavlink_log_info(mavlink_fd, offset_output); + + close(sub_sensor_combined); +} + +void do_gyro_calibration(void) +{ const int calibration_count = 3000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; int calibration_counter = 0; - float gyro_offset[3] = {0, 0, 0}; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; while (calibration_counter < calibration_count) { @@ -234,6 +394,10 @@ void do_gyro_calibration(void) gyro_offset[1] += raw.gyro_raw[1]; gyro_offset[2] += raw.gyro_raw[2]; calibration_counter++; + } else { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + return; } } @@ -325,12 +489,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // /* preflight calibration */ case MAV_CMD_PREFLIGHT_CALIBRATION: { - if (cmd->param1 == 1.0) { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); do_gyro_calibration(); result = MAV_RESULT_ACCEPTED; + handled = true; + } - } else { + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); + do_mag_calibration(); + result = MAV_RESULT_ACCEPTED; + } + + /* none found */ + if (!handled) { fprintf(stderr, "[commander] refusing unsupported calibration request\n"); mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request"); result = MAV_RESULT_UNSUPPORTED; @@ -342,7 +519,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta case MAV_CMD_PREFLIGHT_STORAGE: { /* Read all parameters from EEPROM to RAM */ - if (cmd->param1 == 0.0) { + if (((int)cmd->param1) == 0) { if (OK == get_params_from_eeprom(global_data_parameter_storage)) { printf("[commander] Loaded EEPROM params in RAM\n"); @@ -357,7 +534,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* Write all parameters from RAM to EEPROM */ - } else if (cmd->param1 == 1.0) { + } else if (((int)cmd->param1) == 1) { if (OK == store_params_in_eeprom(global_data_parameter_storage)) { printf("[commander] RAM params written to EEPROM\n"); @@ -574,7 +751,7 @@ int commander_main(int argc, char *argv[]) /* create pthreads */ pthread_attr_t command_handling_attr; pthread_attr_init(&command_handling_attr); - pthread_attr_setstacksize(&command_handling_attr, 3072); + pthread_attr_setstacksize(&command_handling_attr, 4096); pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL); // pthread_attr_t subsystem_info_attr; @@ -608,13 +785,16 @@ int commander_main(int argc, char *argv[]) /* Subscribe to RC data */ int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - struct rc_channels_s rc = {0}; + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps = {0}; + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors = {0}; + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); uint8_t vehicle_state_previous = current_status.state_machine; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 3343b33f49..7e8eac4531 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -300,17 +300,17 @@ int sensors_main(int argc, char *argv[]) unsigned int adc_fail_count = 0; unsigned int adc_success_count = 0; - ssize_t ret_gyro; - ssize_t ret_accelerometer; - ssize_t ret_magnetometer; - ssize_t ret_barometer; - ssize_t ret_adc; - int nsamples_adc; + ssize_t ret_gyro; + ssize_t ret_accelerometer; + ssize_t ret_magnetometer; + ssize_t ret_barometer; + ssize_t ret_adc; + int nsamples_adc; int16_t buf_gyro[3]; int16_t buf_accelerometer[3]; - int16_t buf_magnetometer[3]; - float buf_barometer[3]; + int16_t buf_magnetometer[7]; + float buf_barometer[3]; int16_t mag_offset[3] = {0, 0, 0}; int16_t acc_offset[3] = {0, 0, 0}; @@ -335,7 +335,7 @@ int sensors_main(int argc, char *argv[]) float battery_voltage_conversion; battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION]; - if (-1.0f == battery_voltage_conversion) { + if (-1 == (int)battery_voltage_conversion) { /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; } @@ -387,11 +387,13 @@ int sensors_main(int argc, char *argv[]) publishing = true; /* advertise the rc topic */ - struct rc_channels_s rc = {0}; + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); /* subscribe to system status */ - struct vehicle_status_s vstatus = {0}; + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -562,6 +564,13 @@ int sensors_main(int argc, char *argv[]) /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); + /* start calibration mode if requested */ + if (raw.magnetometer_mode == MAGNETOMETER_MODE_NORMAL && vstatus.preflight_mag_calibration) { + ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); + } else if (raw.magnetometer_mode != MAGNETOMETER_MODE_NORMAL && !vstatus.preflight_mag_calibration) { + ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); + } + ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); int magtime = hrt_absolute_time() - start_mag; @@ -765,6 +774,9 @@ int sensors_main(int argc, char *argv[]) raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f; raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f; + /* store mode */ + raw.magnetometer_mode = buf_magnetometer[3]; + raw.magnetometer_raw_counter++; } diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index ecf5ea81d6..8fae788585 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -51,6 +51,12 @@ * @{ */ +enum MAGNETOMETER_MODE { + MAGNETOMETER_MODE_NORMAL = 0, + MAGNETOMETER_MODE_POSITIVE_BIAS, + MAGNETOMETER_MODE_NEGATIVE_BIAS +}; + /** * Sensor readings in raw and SI-unit form. * @@ -71,25 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ + uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ + + int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ + uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */ + float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ + + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ + uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */ + float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ + int accelerometer_mode; /**< Accelerometer measurement mode */ + float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ - uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */ - float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ - int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ - uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */ - float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ - int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */ - float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */ - uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */ - float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ - float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ - float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ - float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ - float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ - uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */ - uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ - bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ + int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */ + float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */ + int magnetometer_mode; /**< Magnetometer measurement mode */ + float magnetometer_range_ga; /**< ± measurement range in Gauss */ + float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ + uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */ + + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ + float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ + float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ + float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ + float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ + uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */ + uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ + bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ }; diff --git a/nuttx/configs/px4fmu/include/drv_hmc5883l.h b/nuttx/configs/px4fmu/include/drv_hmc5883l.h index 8dc9b8a93d..741c3e1544 100644 --- a/nuttx/configs/px4fmu/include/drv_hmc5883l.h +++ b/nuttx/configs/px4fmu/include/drv_hmc5883l.h @@ -35,15 +35,6 @@ * Driver for the ST HMC5883L gyroscope */ -/* IMPORTANT NOTES: - * - * SPI max. clock frequency: 10 Mhz - * CS has to be high before transfer, - * go low right before transfer and - * go high again right after transfer - * - */ - #include #define _HMC5883LBASE 0x6100 @@ -76,6 +67,13 @@ #define HMC5883L_RANGE_2_50GA (3 << 5) #define HMC5883L_RANGE_4_00GA (4 << 5) +/* + * Set the sensor measurement mode. + */ +#define HMC5883L_MODE_NORMAL (0 << 0) +#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) +#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) + /* * Sets the address of a shared HMC5883L_buffer * structure that is maintained by the driver. @@ -83,7 +81,7 @@ * If zero is passed as the address, disables * the buffer updating. */ -#define HMC5883L_SETBUFFER HMC5883LC(3) +#define HMC5883L_SETBUFFER HMC5883LC(3) struct hmc5883l_buffer { uint32_t size; /* number of entries in the samples[] array */ @@ -95,6 +93,8 @@ struct hmc5883l_buffer { } samples[]; }; -#define HMC5883L_RESET HMC5883LC(4) +#define HMC5883L_RESET HMC5883LC(4) +#define HMC5883L_CALIBRATION_ON HMC5883LC(5) +#define HMC5883L_CALIBRATION_OFF HMC5883LC(6) extern int hmc5883l_attach(struct i2c_dev_s *i2c); diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c index 2a6d04306a..df6e26d4b6 100644 --- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c +++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c @@ -29,7 +29,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/* +/** + * @file drv_hmc5883l.c * Driver for the Honeywell/ST HMC5883L MEMS magnetometer */ @@ -108,6 +109,7 @@ struct hmc5883l_dev_s uint8_t rate; struct hmc5883l_buffer *buffer; }; +static bool hmc5883l_calibration_enabled = false; static int hmc5883l_write_reg(uint8_t address, uint8_t data); static int hmc5883l_read_reg(uint8_t address); @@ -169,6 +171,24 @@ hmc5883l_set_rate(uint8_t rate) return !(hmc5883l_read_reg(ADDR_CONF_A) == write_rate); } +static int +hmc5883l_set_mode(uint8_t mode) +{ + // I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7); + // /* mask out illegal bit positions */ + // uint8_t write_mode = mode & 0x03; + // /* immediately return if user supplied invalid value */ + // if (write_mode != mode) return EINVAL; + // /* set mode */ + // write_mode |= hmc5883l_read_reg(ADDR_CONF_A); + // /* set remaining bits to a sane value */ + // write_mode |= HMC5883L_AVERAGING_8; + // /* write to device */ + // hmc5883l_write_reg(ADDR_CONF_A, write_mode); + // /* return 0 if register value is now written value, 1 if unchanged */ + // return !(hmc5883l_read_reg(ADDR_CONF_A) == write_mode); +} + static bool read_values(int16_t *data) { @@ -204,7 +224,8 @@ read_values(int16_t *data) int hmc_status = hmc5883l_read_reg(ADDR_STATUS); if (hmc_status < 0) { - if (hmc_status == ETIMEDOUT) hmc5883l_reset(); + //if (hmc_status == ETIMEDOUT) + hmc5883l_reset(); ret = hmc_status; } else @@ -215,12 +236,12 @@ read_values(int16_t *data) } else { - if (ret == ETIMEDOUT) hmc5883l_reset(); + if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset(); } } else { - if (ret == ETIMEDOUT) hmc5883l_reset(); + if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset(); } if (ret != OK) @@ -236,9 +257,11 @@ read_values(int16_t *data) data[0] = ((hmc_report.x & 0x00FF) << 8) | ((hmc_report.x & 0xFF00) >> 8); data[1] = ((hmc_report.y & 0x00FF) << 8) | ((hmc_report.y & 0xFF00) >> 8); data[2] = ((hmc_report.z & 0x00FF) << 8) | ((hmc_report.z & 0xFF00) >> 8); + // XXX TODO + // write mode, range and lp-frequency enum values into data[3]-[6] if ((hmc_report.status & STATUS_REG_DATA_READY) > 0) { - ret = 6; + ret = 14; } else { ret = -EAGAIN; } @@ -252,7 +275,7 @@ static ssize_t hmc5883l_read(struct file *filp, char *buffer, size_t buflen) { /* if the buffer is large enough, and data are available, return success */ - if (buflen >= 6) { + if (buflen >= 14) { return read_values((int16_t *)buffer); } @@ -267,20 +290,30 @@ hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg) int result = ERROR; switch (cmd) { - case HMC5883L_SETRATE: + case HMC5883L_SETRATE: result = hmc5883l_set_rate(arg); break; - case HMC5883L_SETRANGE: - result = hmc5883l_set_range(arg); - break; + case HMC5883L_SETRANGE: + result = hmc5883l_set_range(arg); + break; + + case HMC5883L_CALIBRATION_ON: + hmc5883l_calibration_enabled = true; + result = OK; + break; + + case HMC5883L_CALIBRATION_OFF: + hmc5883l_calibration_enabled = false; + result = OK; + break; // // case HMC5883L_SETBUFFER: // hmc5883l_dev.buffer = (struct hmc5883l_buffer *)arg; // result = 0; // break; - case HMC5883L_RESET: + case HMC5883L_RESET: result = hmc5883l_reset(); break; } @@ -297,12 +330,6 @@ int hmc5883l_reset() up_i2cuninitialize(hmc5883l_dev.i2c); hmc5883l_dev.i2c = up_i2cinitialize(2); I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000); - // up_i2creset(hmc5883l_dev.i2c); - //I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7); - //hmc5883l_set_range(HMC5883L_RANGE_0_88GA); - //hmc5883l_set_rate(HMC5883L_RATE_75HZ); - /* set device into single mode, start measurement */ - //ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); return ret; }