zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Minor fixes to HMC driver, mag calibration done

sbg
Lorenz Meier 13 years ago
parent
139cd09176
commit
96b348af9f
5 changed files with 301 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 202
      apps/commander/commander.c
  2. 34
      apps/sensors/sensors.c
  3. 50
      apps/uORB/topics/sensor_combined.h
  4. 22
      nuttx/configs/px4fmu/include/drv_hmc5883l.h
  5. 61
      nuttx/configs/px4fmu/src/drv_hmc5833l.c

202
apps/commander/commander.c
Unescape View File

@ -6,7 +6,6 @@ @@ -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; @@ -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 @@ -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;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) @@ -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 @@ -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 @@ -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 @@ -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[]) @@ -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[]) @@ -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;

34
apps/sensors/sensors.c
Unescape View File

@ -300,17 +300,17 @@ int sensors_main(int argc, char *argv[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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++;
}

50
apps/uORB/topics/sensor_combined.h
Unescape View File

@ -51,6 +51,12 @@ @@ -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 { @@ -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 */
};

22
nuttx/configs/px4fmu/include/drv_hmc5883l.h
Unescape View File

@ -35,15 +35,6 @@ @@ -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 @@ @@ -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 @@ @@ -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 { @@ -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);

61
nuttx/configs/px4fmu/src/drv_hmc5833l.c
Unescape View File

@ -29,7 +29,8 @@ @@ -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 @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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() @@ -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;
}

Write Preview
Loading…
Cancel
Save