diff options
author | Doug Weibel <deweibel@gmail.com> | 2012-10-29 18:24:47 -0600 |
---|---|---|
committer | Doug Weibel <deweibel@gmail.com> | 2012-10-29 18:24:47 -0600 |
commit | 09ec869ae903c6d60bf2f53b314a727fd54e19f6 (patch) | |
tree | 44584f6f3408735468f3e3c9f7ed64aa2c4ec4fd | |
parent | db8d369c55d091b988a054ad39fd323e945a9bb0 (diff) | |
parent | 574eb96a2ebafeb03d2933c68cb7f7b60269601a (diff) | |
download | px4-firmware-09ec869ae903c6d60bf2f53b314a727fd54e19f6.tar.gz px4-firmware-09ec869ae903c6d60bf2f53b314a727fd54e19f6.tar.bz2 px4-firmware-09ec869ae903c6d60bf2f53b314a727fd54e19f6.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into FW_control
-rwxr-xr-x | ROMFS/scripts/rcS | 2 | ||||
-rw-r--r-- | apps/commander/commander.c | 25 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 49 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 3 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 4 | ||||
-rw-r--r-- | apps/systemcmds/eeprom/24xxxx_mtd.c | 27 | ||||
-rw-r--r-- | apps/systemcmds/eeprom/eeprom.c | 14 |
7 files changed, 120 insertions, 4 deletions
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 409bde33b..b5fbfe0f5 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -46,6 +46,8 @@ if [ -f /fs/microsd/etc/rc ] then echo "[init] reading /fs/microsd/etc/rc" sh /fs/microsd/etc/rc +else + echo "[init] script /fs/microsd/etc/rc not present" fi # diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a7550b54b..d93a48e31 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -360,6 +360,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) float *y = malloc(sizeof(float) * calibration_maxcount); float *z = malloc(sizeof(float) * calibration_maxcount); + tune_confirm(); + sleep(2); + tune_confirm(); + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -504,6 +508,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + } else { mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); } @@ -607,6 +618,12 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); } @@ -721,6 +738,12 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) //sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "[commander] accel calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); } @@ -740,7 +763,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta uint8_t result = MAV_RESULT_UNSUPPORTED; /* announce command handling */ - ioctl(buzzer, TONE_SET_ALARM, 1); + tune_confirm(); /* supported command handling start */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index f9079c3ff..6fefbfafc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,8 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <float.h> + /* * HMC5883 internal constants and data structures. */ @@ -159,6 +161,10 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + /* status reporting */ + bool _sensor_ok; /**< sensor was found and reports ok */ + bool _calibrated; /**< the calibration is valid */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -272,6 +278,13 @@ private: */ float meas_to_float(uint8_t in[2]); + /** + * Check the current calibration and update device status + * + * @return 0 if calibration is ok, 1 else + */ + int check_calibration(); + }; /* helper macro for handling report buffer indices */ @@ -295,7 +308,9 @@ HMC5883::HMC5883(int bus) : _mag_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _sensor_ok(false), + _calibrated(false) { // enable debug() calls _debug_enabled = true; @@ -351,6 +366,8 @@ HMC5883::init() set_range(_range_ga); ret = OK; + /* sensor is ok, but not calibrated */ + _sensor_ok = true; out: return ret; } @@ -1000,6 +1017,36 @@ out: return ret; } +int HMC5883::check_calibration() +{ + bool scale_valid, offset_valid; + + if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) && + (-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) && + (-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) { + /* scale is different from one */ + scale_valid = true; + } else { + scale_valid = false; + } + + if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + /* offset is different from zero */ + offset_valid = true; + } else { + offset_valid = false; + } + + if (_calibrated && !(offset_valid && scale_valid)) { + warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ", + (offset_valid) ? "" : "offset invalid."); + _calibrated = false; + // XXX Notify system via uORB + } +} + int HMC5883::set_excitement(unsigned enable) { int ret; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 5e8c5746c..892ec975a 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -130,6 +130,9 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); +PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); +PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 54d2f6a0b..9522bd31a 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,6 +1030,10 @@ Sensors::task_main() manual_control.pitch = 0.0f; manual_control.yaw = 0.0f; manual_control.throttle = 0.0f; + manual_control.aux1_cam_pan_flaps = 0.0f; + manual_control.aux2_cam_tilt = 0.0f; + manual_control.aux3_cam_zoom = 0.0f; + manual_control.aux4_cam_roll = 0.0f; _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 79149caa0..781b01065 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -502,6 +502,33 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); } + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + /* Return the implementation-specific state structure as the MTD device */ fvdbg("Return %p\n", priv); diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index 19a14aa02..b4257cda9 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -118,9 +118,19 @@ eeprom_attach(void) if (i2c == NULL) errx(1, "failed to locate I2C bus"); - /* start the MTD driver */ - eeprom_mtd = at24c_initialize(i2c); + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + eeprom_mtd = at24c_initialize(i2c); + if (eeprom_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + /* if last attempt is still unsuccessful, abort */ if (eeprom_mtd == NULL) errx(1, "failed to initialize EEPROM driver"); |