aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/commander/commander.c202
-rw-r--r--apps/sensors/sensors.c34
-rw-r--r--apps/uORB/topics/sensor_combined.h50
-rw-r--r--nuttx/configs/px4fmu/include/drv_hmc5883l.h22
-rw-r--r--nuttx/configs/px4fmu/src/drv_hmc5833l.c61
5 files changed, 301 insertions, 68 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f342298ee..1c83dd3f3 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -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 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)
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 3343b33f4..7e8eac453 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 ecf5ea81d..8fae78858 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 8dc9b8a93..741c3e154 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 <sys/ioctl.h>
#define _HMC5883LBASE 0x6100
@@ -77,13 +68,20 @@
#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.
*
* 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 2a6d04306..df6e26d4b 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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 08:37:13 +0000