Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Minor fixes to HMC driver, mag calibration done

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-05 22:51:31 +02:00
parent 139cd09176
commit 96b348af9f
5 changed files with 301 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+191 -11
View File
@@ -6,7 +6,6 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
*
* 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 cal_bsort(int16_t a[], int n)
{
int i,j,t;
for(i=0;i<n-1;i++)
{
for(j=0;j<n-i-1;j++)
{
if(a[j]>a[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;
apps/sensors/sensors.c
+23 -11
View File
@@ -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++;
}
apps/uORB/topics/sensor_combined.h
+32 -18
View File
@@ -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 */
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 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 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 */
};
nuttx/configs/px4fmu/include/drv_hmc5883l.h
+11 -11
View File
@@ -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 <sys/ioctl.h>
#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);
nuttx/configs/px4fmu/src/drv_hmc5833l.c
+44 -17
View File
@@ -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;
}