From 41ec41cf8cc16309cf6f7e949d3ddad78e5f44a2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 6 May 2013 18:21:56 +0400 Subject: [PATCH 1/2] Accelerometer calibration bugfix --- apps/commander/accelerometer_calibration.c | 14 ++++++-------- apps/commander/accelerometer_calibration.h | 4 ++-- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index 991145d73c..d79dd93dd0 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -232,7 +232,6 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - float accel_len2 = 0.0f; /* EMA time constant in seconds*/ float ema_len = 0.2f; /* set "still" threshold to 0.1 m/s^2 */ @@ -304,30 +303,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - float accel_len = sqrt(accel_len2); - if ( fabs(accel_ema[0] - accel_len) < accel_err_thr && + if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + accel_len) < accel_err_thr && + if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - accel_len) < accel_err_thr && + fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + accel_len) < accel_err_thr && + fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - accel_len) < accel_err_thr ) + fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] if ( fabs(accel_ema[0]) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + accel_len) < accel_err_thr ) + fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h index c0169c2a13..a11cf93d3c 100644 --- a/apps/commander/accelerometer_calibration.h +++ b/apps/commander/accelerometer_calibration.h @@ -1,8 +1,8 @@ /* * accelerometer_calibration.h * - * Created on: 25.04.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin */ #ifndef ACCELEROMETER_CALIBRATION_H_ From f3e6e4bb50d9d5e63ee423a25713d9033adcebf4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 6 May 2013 22:59:45 +0400 Subject: [PATCH 2/2] Update servo arm only on real change. --- apps/drivers/px4fmu/fmu.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index e547245367..761a23c426 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -505,7 +505,11 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - up_pwm_servo_arm(aa.armed && !aa.lockdown); + bool set_armed = aa.armed && !aa.lockdown; + if (set_armed != _armed) { + _armed = set_armed; + up_pwm_servo_arm(set_armed); + } } // see if we have new PPM input data