From a31f30e6bd0226c57a19eb16516cfa4abe234c45 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 18 Aug 2014 12:56:22 +0900 Subject: [PATCH 001/254] Copter: range check ACRO_EXPO to be no more than 1 --- ArduCopter/control_acro.pde | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 0b928adb428..c7975f5da81 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -59,6 +59,11 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int // expo variables float rp_in, rp_in3, rp_out; + // range check expo + if (g.acro_expo > 1.0f) { + g.acro_expo = 1.0f; + } + // roll expo rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; From 33432aa4a816b5be77718b6f7f44366fb01f1895 Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 7 Aug 2014 01:59:19 +1000 Subject: [PATCH 002/254] Copter : Distinguish between EKF and INAV errors --- ArduCopter/defines.h | 8 ++++---- ArduCopter/ekf_check.pde | 18 +++++++++++++----- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 8a19fa343b0..51269ab16be 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -329,8 +329,8 @@ enum FlipState { #define ERROR_SUBSYSTEM_FLIP 13 #define ERROR_SUBSYSTEM_AUTOTUNE 14 #define ERROR_SUBSYSTEM_PARACHUTE 15 -#define ERROR_SUBSYSTEM_EKF_CHECK 16 -#define ERROR_SUBSYSTEM_FAILSAFE_EKF 17 +#define ERROR_SUBSYSTEM_EKFINAV_CHECK 16 +#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17 #define ERROR_SUBSYSTEM_BARO 18 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 @@ -356,8 +356,8 @@ enum FlipState { // parachute failed to deploy because of low altitude #define ERROR_CODE_PARACHUTE_TOO_LOW 2 // EKF check definitions -#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE 2 -#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED 0 +#define ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE 2 +#define ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED 0 // Baro specific error codes #define ERROR_CODE_BARO_GLITCH 2 diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index a69fa73ff47..a4d24be91a4 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -76,10 +76,18 @@ void ekf_check() ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_compass = true; // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE); + Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); +#if AP_AHRS_NAVEKF_AVAILABLE + if (ahrs.have_inertial_nav()) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + } else { + gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); + } +#else + gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); +#endif ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -94,7 +102,7 @@ void ekf_check() if (ekf_check_state.fail_count_compass == 0) { ekf_check_state.bad_compass = false; // log recovery in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED); + Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -126,7 +134,7 @@ static void failsafe_ekf_event() // EKF failsafe event has occurred failsafe.ekf = true; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_OCCURRED); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED); // take action based on flight mode if (mode_requires_GPS(control_mode)) { @@ -149,5 +157,5 @@ static void failsafe_ekf_off_event(void) // clear flag and log recovery failsafe.ekf = false; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_RESOLVED); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); } From 86abf82cc7f11447de405fe407b3a769393599ba Mon Sep 17 00:00:00 2001 From: NullVoxPopuli Date: Mon, 18 Aug 2014 14:25:24 +0900 Subject: [PATCH 003/254] Copter: Added support for V-Shaped and A-Shaped VTail Quadcopter frames Signed-off-by: NullVoxPopuli --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 16 ++++--- libraries/AP_Motors/AP_MotorsMatrix.h | 3 ++ libraries/AP_Motors/AP_MotorsQuad.cpp | 62 ++++++++++++++----------- libraries/AP_Motors/AP_Motors_Class.h | 3 +- 4 files changed, 49 insertions(+), 35 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index ef3c0ab8221..903d7e14a66 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -385,17 +385,21 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc } } -// add_motor using just position and prop direction +// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order) { - // call raw motor set-up method + add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order); +} + +// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames) +void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order) +{ add_motor_raw( motor_num, - cosf(radians(angle_degrees + 90)), // roll factor - cosf(radians(angle_degrees)), // pitch factor - yaw_factor, // yaw factor + cosf(radians(roll_factor_in_degrees + 90)), + cosf(radians(pitch_factor_in_degrees)), + yaw_factor, testing_order); - } // remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index eac0b8cb89a..389688df79d 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -49,6 +49,9 @@ class AP_MotorsMatrix : public AP_Motors { // add_motor using just position and yaw_factor (or prop direction) void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); + // add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction + void add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order); + // remove_motor void remove_motor(int8_t motor_num); diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index 8a831f265c4..81ad765601b 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -49,39 +49,45 @@ void AP_MotorsQuad::setup_motors() add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + }else if(_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) { + /* + Tested with: Lynxmotion Hunter Vtail 400 + - inverted rear outward blowing motors (at a 40 degree angle) + - should also work with non-inverted rear outward blowing motors + - no roll in rear motors + - no yaw in front motors + - should fly like some mix between a tricopter and X Quadcopter - } else if (_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) { - /* Lynxmotion Hunter Vtail 400/500 + Roll control comes only from the front motors, Yaw control only from the rear motors. + Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm. - Roll control comes only from the front motors, Yaw control only from the rear motors - roll factor is measured by the angle perpendicular to that of the prop arm to the roll axis (x) - pitch factor is measured by the angle perpendicular to the prop arm to the pitch axis (y) + Note: if we want the front motors to help with yaw, + motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle + motors 3's yaw factor should be changed to -sin(radians(40)) + */ - assumptions: - 20 20 - \ / 3_____________1 - \ / | - \ / | - 40 \/ 40 20 | 20 - Tail / \ - 2 4 + add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1); + add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4); + add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + } else if (_flags.frame_orientation == AP_MOTORS_ATAIL_FRAME) { + /* + The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference: + - The Yaw factors are reversed, because the rear motors are facing different directions - All angles measured from their closest axis - - Note: if we want the front motors to help with yaw, - motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle - motors 3's yaw factor should be changed to -sin(radians(40)) - */ - - // front right: 70 degrees right of roll axis, 20 degrees up of pitch axis, no yaw - add_motor_raw(AP_MOTORS_MOT_1, cosf(radians(160)), cosf(radians(-70)), 0, 1); - // back left: no roll, 70 degrees down of pitch axis, full yaw - add_motor_raw(AP_MOTORS_MOT_2, 0, cosf(radians(160)), AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); - // front left: 70 degrees left of roll axis, 20 degrees up of pitch axis, no yaw - add_motor_raw(AP_MOTORS_MOT_3, cosf(radians(20)), cosf(radians(70)), 0, 4); - // back right: no roll, 70 degrees down of pitch axis, full yaw - add_motor_raw(AP_MOTORS_MOT_4, 0, cosf(radians(-160)), AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + With V-Shaped VTails, the props make a V-Shape when spinning, but with + A-Shaped VTails, the props make an A-Shape when spinning. + - Rear thrust on a V-Shaped V-Tail Quad is outward + - Rear thrust on an A-Shaped V-Tail Quad is inward + Still functions the same as the V-Shaped VTail mixing below: + - Yaw control is entirely in the rear motors + - Roll is is entirely in the front motors + */ + add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1); + add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4); + add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); }else{ // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 0dbc57e2b90..befa01a1d26 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -39,6 +39,7 @@ #define AP_MOTORS_V_FRAME 2 #define AP_MOTORS_H_FRAME 3 // same as X frame but motors spin in opposite direction #define AP_MOTORS_VTAIL_FRAME 4 // Lynxmotion Hunter VTail 400/500 +#define AP_MOTORS_ATAIL_FRAME 5 // A-Shaped VTail Quads #define AP_MOTORS_NEW_PLUS_FRAME 10 // NEW frames are same as original 4 but with motor orders changed to be clockwise from the front #define AP_MOTORS_NEW_X_FRAME 11 #define AP_MOTORS_NEW_V_FRAME 12 @@ -170,7 +171,7 @@ class AP_Motors { AP_Int8 _throttle_curve_enabled; // enable throttle curve AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range - AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min + AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min // internal variables RC_Channel& _rc_roll; // roll input in from users is held in servo_out From 2183d2f4f3a8eb99aa7d2663dc4f9bf4ac905f0f Mon Sep 17 00:00:00 2001 From: NullVoxPopuli Date: Mon, 18 Aug 2014 14:25:37 +0900 Subject: [PATCH 004/254] Copter: add A-Tail to FRAME parameter description --- ArduCopter/Parameters.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 136c4cc1961..779e5c520ca 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -381,7 +381,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: FRAME // @DisplayName: Frame Orientation (+, X or V) // @Description: Controls motor mixing for multicopters. Not used for Tri or Traditional Helicopters. - // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 10:Y6B (New) + // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B (New) // @User: Standard GSCALAR(frame_orientation, "FRAME", AP_MOTORS_X_FRAME), @@ -993,7 +993,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Group: BRD_ // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp - GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), + GOBJECT(BoardConfig, "BRD_", AP_BoardConfig), #if SPRAYER == ENABLED // @Group: SPRAY_ @@ -1131,7 +1131,7 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = { static void load_parameters(void) { if (!AP_Param::check_var_info()) { - cliSerial->printf_P(PSTR("Bad var table\n")); + cliSerial->printf_P(PSTR("Bad var table\n")); hal.scheduler->panic(PSTR("Bad var table")); } From c395a6657a8339fb7c05984f01b009137277615b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Aug 2014 21:45:11 +1000 Subject: [PATCH 005/254] DataFlash: moved UBX logging headers to DataFlash --- libraries/DataFlash/DataFlash.h | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index a77c8866c0f..2864478ffcd 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -392,6 +392,28 @@ struct PACKED log_TERRAIN { uint16_t loaded; }; +/* + UBlox logging + */ +struct PACKED log_Ubx1 { + LOG_PACKET_HEADER; + uint32_t timestamp; + uint8_t instance; + uint16_t noisePerMS; + uint8_t jamInd; + uint8_t aPower; +}; + +struct PACKED log_Ubx2 { + LOG_PACKET_HEADER; + uint32_t timestamp; + uint8_t instance; + int8_t ofsI; + uint8_t magI; + int8_t ofsQ; + uint8_t magQ; +}; + // messages for all boards #define LOG_BASE_STRUCTURES \ { LOG_FORMAT_MSG, sizeof(log_Format), \ @@ -440,7 +462,11 @@ struct PACKED log_TERRAIN { { LOG_EKF4_MSG, sizeof(log_EKF4), \ "EKF4","IcccccccbbBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,DS" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ - "TERR","IBLLHffHH","TimeMS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" } + "TERR","IBLLHffHH","TimeMS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ + { LOG_UBX1_MSG, sizeof(log_Ubx1), \ + "UBX1", "IBHBB", "TimeMS,Instance,noisePerMS,jamInd,aPower" }, \ + { LOG_UBX2_MSG, sizeof(log_Ubx2), \ + "UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" } #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES @@ -474,6 +500,8 @@ struct PACKED log_TERRAIN { #define LOG_CAMERA_MSG 148 #define LOG_IMU3_MSG 149 #define LOG_TERRAIN_MSG 150 +#define LOG_UBX1_MSG 151 +#define LOG_UBX2_MSG 152 // message types 200 to 210 reversed for GPS driver use // message types 211 to 220 reversed for autotune use From 57956dbda239d9b4806d9d3077471122ffc57ead Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Aug 2014 21:45:32 +1000 Subject: [PATCH 006/254] AP_GPS: moved UBX log headers to DataFlash headers were not always being written --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 48 ++----------------------------- libraries/AP_GPS/AP_GPS_UBLOX.h | 3 -- 2 files changed, 2 insertions(+), 49 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index e6e8552dd1f..0cda3ee0095 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -44,8 +44,6 @@ extern const AP_HAL::HAL& hal; #define UBLOX_HW_LOGGING 0 #endif -bool AP_GPS_UBLOX::logging_started = false; - AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port), _step(0), @@ -249,53 +247,13 @@ AP_GPS_UBLOX::read(void) // Private Methods ///////////////////////////////////////////////////////////// #if UBLOX_HW_LOGGING -#define LOG_MSG_UBX1 200 -#define LOG_MSG_UBX2 201 - -struct PACKED log_Ubx1 { - LOG_PACKET_HEADER; - uint32_t timestamp; - uint8_t instance; - uint16_t noisePerMS; - uint8_t jamInd; - uint8_t aPower; -}; - -struct PACKED log_Ubx2 { - LOG_PACKET_HEADER; - uint32_t timestamp; - uint8_t instance; - int8_t ofsI; - uint8_t magI; - int8_t ofsQ; - uint8_t magQ; -}; - -static const struct LogStructure ubx_log_structures[] PROGMEM = { - { LOG_MSG_UBX1, sizeof(log_Ubx1), - "UBX1", "IBHBB", "TimeMS,Instance,noisePerMS,jamInd,aPower" }, - { LOG_MSG_UBX2, sizeof(log_Ubx2), - "UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" } -}; - -void AP_GPS_UBLOX::write_logging_headers(void) -{ - if (!logging_started) { - logging_started = true; - gps._DataFlash->AddLogFormats(ubx_log_structures, 2); - } -} - void AP_GPS_UBLOX::log_mon_hw(void) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } - // log mon_hw message - write_logging_headers(); - struct log_Ubx1 pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_UBX1), + LOG_PACKET_HEADER_INIT(LOG_UBX1_MSG), timestamp : hal.scheduler->millis(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, @@ -315,11 +273,9 @@ void AP_GPS_UBLOX::log_mon_hw2(void) if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } - // log mon_hw message - write_logging_headers(); struct log_Ubx2 pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_UBX2), + LOG_PACKET_HEADER_INIT(LOG_UBX2_MSG), timestamp : hal.scheduler->millis(), instance : state.instance, ofsI : _buffer.mon_hw2.ofsI, diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index fec84200597..a9d96b7aedf 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -256,9 +256,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend bool _new_speed:1; bool need_rate_update:1; - // have we written the logging headers to DataFlash? - static bool logging_started; - uint8_t _disable_counter; // Buffer parse & GPS state update From ecd73413db5dd1ff688881daa50e2bba9409df9f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Aug 2014 23:11:16 +1000 Subject: [PATCH 007/254] Copter: set GPS non-blocking the new GPS driver only ever needs a non-blocking port --- ArduCopter/system.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index ee00e4a0d36..89c0f5df540 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -288,6 +288,7 @@ static void init_ardupilot() // mid-flight, so set the serial ports non-blocking once we are // ready to fly hal.uartA->set_blocking_writes(false); + hal.uartB->set_blocking_writes(false); hal.uartC->set_blocking_writes(false); if (hal.uartD != NULL) { hal.uartD->set_blocking_writes(false); From 54af047b87f2fea060958d5f32c77e65d8b470df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Aug 2014 09:30:08 +1000 Subject: [PATCH 008/254] HAL_PX4: prevent read past end of buffer --- libraries/AP_HAL_PX4/UARTDriver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index e92c14a8ac0..fd0219381c3 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -503,7 +503,7 @@ void PX4UARTDriver::_timer_tick(void) // split into two writes uint16_t n1 = _writebuf_size - _writebuf_head; int ret = _write_fd(&_writebuf[_writebuf_head], n1); - if (ret == n1 && n != n1) { + if (ret == n1 && n > n1) { _write_fd(&_writebuf[_writebuf_head], n - n1); } } @@ -523,7 +523,7 @@ void PX4UARTDriver::_timer_tick(void) uint16_t n1 = _readbuf_size - _readbuf_tail; assert(_readbuf_tail+n1 <= _readbuf_size); int ret = _read_fd(&_readbuf[_readbuf_tail], n1); - if (ret == n1 && n != n1) { + if (ret == n1 && n > n1) { assert(_readbuf_tail+(n-n1) <= _readbuf_size); _read_fd(&_readbuf[_readbuf_tail], n - n1); } From 308c90f138fd671ab3c8ccc0f3a7ff48537d7614 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Aug 2014 09:30:19 +1000 Subject: [PATCH 009/254] HAL_VRBrain: prevent read past end of buffer --- libraries/AP_HAL_VRBRAIN/UARTDriver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_VRBRAIN/UARTDriver.cpp b/libraries/AP_HAL_VRBRAIN/UARTDriver.cpp index c4fbd15cc53..ad0f0f4fb58 100644 --- a/libraries/AP_HAL_VRBRAIN/UARTDriver.cpp +++ b/libraries/AP_HAL_VRBRAIN/UARTDriver.cpp @@ -503,7 +503,7 @@ void VRBRAINUARTDriver::_timer_tick(void) // split into two writes uint16_t n1 = _writebuf_size - _writebuf_head; int ret = _write_fd(&_writebuf[_writebuf_head], n1); - if (ret == n1 && n != n1) { + if (ret == n1 && n > n1) { _write_fd(&_writebuf[_writebuf_head], n - n1); } } @@ -523,7 +523,7 @@ void VRBRAINUARTDriver::_timer_tick(void) uint16_t n1 = _readbuf_size - _readbuf_tail; assert(_readbuf_tail+n1 <= _readbuf_size); int ret = _read_fd(&_readbuf[_readbuf_tail], n1); - if (ret == n1 && n != n1) { + if (ret == n1 && n > n1) { assert(_readbuf_tail+(n-n1) <= _readbuf_size); _read_fd(&_readbuf[_readbuf_tail], n - n1); } From 9c79f9b38d3c34780c954d2f75a28062a7885150 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 19 Aug 2014 12:51:27 +0900 Subject: [PATCH 010/254] Copter: add ACRO_EXPO param values --- ArduCopter/Parameters.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 779e5c520ca..96522ae61ae 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -589,7 +589,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: ACRO_EXPO // @DisplayName: Acro Expo // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges - // @Values: 0:Disabled,0.2:Low,0.3:Medium,0.4:High + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High // @User: Advanced GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT), From 2d02d8d15fc3997e3ccc2dc0fc6775dfd050e225 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Aug 2014 14:38:53 +0900 Subject: [PATCH 011/254] Copter: remove inav check the EKF check works reliably but attempts to check the inertial nav for errors has not been successful. I could not find a way to reliably catch flyaways without also introducing false positives (and thus unwanted LANDings) --- ArduCopter/ekf_check.pde | 54 ++++++++++++++-------------------------- 1 file changed, 19 insertions(+), 35 deletions(-) diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index a4d24be91a4..bb4ae2ac630 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -34,8 +34,9 @@ static struct { // should be called at 10hz void ekf_check() { - // return immediately if motors are not armed, ekf check is disabled, no inertial-nav position yet or usb is connected - if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !inertial_nav.position_ok() || ap.usb_connected) { +#if AP_AHRS_NAVEKF_AVAILABLE + // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected + if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !ahrs.have_inertial_nav() || ap.usb_connected) { ekf_check_state.fail_count_compass = 0; ekf_check_state.bad_compass = 0; AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; @@ -43,26 +44,14 @@ void ekf_check() return; } - // variances - float compass_variance = 0; - float vel_variance = 9.0; // default set high to enable failsafe trigger if not using EKF - -#if AP_AHRS_NAVEKF_AVAILABLE - if (ahrs.have_inertial_nav()) { - // use EKF to get variance - float posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); - compass_variance = magVar.length(); - } else { - // use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances - compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION; - } -#else - // use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances - compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION; -#endif + // use EKF to get variance + float posVar, hgtVar, tasVar; + Vector3f magVar; + Vector2f offset; + float compass_variance; + float vel_variance; + ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); + compass_variance = magVar.length(); // compare compass and velocity variance vs threshold if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) { @@ -79,15 +68,7 @@ void ekf_check() Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { -#if AP_AHRS_NAVEKF_AVAILABLE - if (ahrs.have_inertial_nav()) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); - } else { - gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); - } -#else - gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); -#endif + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -112,13 +93,15 @@ void ekf_check() // set AP_Notify flags AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; - // To-Do: add check for althold when vibrations are high // To-Do: add ekf variances to extended status - // To-Do: add counter measures to try and recover from bad EKF - // To-Do: add check into GPS position_ok() to return false if ekf xy not healthy? - // To-Do: ensure it compiles for AVR +#else + // if no EKF available then never trigger failure + ekf_check_state.bad_compass = 0; + failsafe.ekf = false; +#endif } +#if AP_AHRS_NAVEKF_AVAILABLE // failsafe_ekf_event - perform ekf failsafe static void failsafe_ekf_event() { @@ -159,3 +142,4 @@ static void failsafe_ekf_off_event(void) failsafe.ekf = false; Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); } +#endif From deaffecbf5fa2688b62ef94f0ed4bf15b950d076 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 14:08:32 -0400 Subject: [PATCH 012/254] AC_AttitudeControl_Heli: Create Flybar Passthrough flag which will be used for control pass-through. --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 4fe6aa4a28f..445fb4fbfcb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -47,10 +47,11 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { // To-Do: move these limits flags into the heli motors class struct AttControlHeliFlags { - uint8_t limit_roll : 1; // 1 if we have requested larger roll angle than swash can physically move - uint8_t limit_pitch : 1; // 1 if we have requested larger pitch angle than swash can physically move - uint8_t limit_yaw : 1; // 1 if we have requested larger yaw angle than tail servo can physically move - uint8_t leaky_i : 1; // 1 if we should use leaky i term for body-frame rate to motor stage + uint8_t limit_roll : 1; // 1 if we have requested larger roll angle than swash can physically move + uint8_t limit_pitch : 1; // 1 if we have requested larger pitch angle than swash can physically move + uint8_t limit_yaw : 1; // 1 if we have requested larger yaw angle than tail servo can physically move + uint8_t leaky_i : 1; // 1 if we should use leaky i term for body-frame rate to motor stage + uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots input directly to swash-plate } _flags_heli; // From ede4b567f2d9f64164fd29eb95a7f0d7b055114e Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 15:04:56 -0400 Subject: [PATCH 013/254] TradHeli: Add pointer for pilot roll/pitch inputs to attitude_control constructor. To be used for flybar passthrough. --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 298fe7880cf..cb024d8ff50 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -648,7 +648,7 @@ static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch, baro_glitch); //////////////////////////////////////////////////////////////////////////////// #if FRAME_CONFIG == HELI_FRAME AC_AttitudeControl_Heli attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, - g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); + g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw, g.rc_1, g.rc_2); #else AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); From 244d38138ed36b8e5c5c25b014a5b25fb77f09fa Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 15:49:48 -0400 Subject: [PATCH 014/254] AC_AttitudeControl_Heli: Add passthrough_to_motor_roll_pitch function. --- .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 14 +++++++++++++- .../AC_AttitudeControl/AC_AttitudeControl_Heli.h | 14 ++++++++++++-- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index f9ab49a534b..19097129530 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -71,7 +71,11 @@ void AC_AttitudeControl_Heli::rate_controller_run() { // call rate controllers and send output to motors object // To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library? - rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y); + if (_flags_heli.flybar_passthrough){ + passthrough_to_motor_roll_pitch(); + }else{ + rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y); + } _motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z)); } @@ -241,6 +245,14 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter */ } +// passthrough_to_motor_roll_pitch - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode +void AC_AttitudeControl_Heli::passthrough_to_motor_roll_pitch() +{ + // output to motors + _motors.set_roll(_rc_roll.control_in); + _motors.set_pitch(_rc_pitch.control_in); +} + // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 445fb4fbfcb..4642b804e3e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -22,11 +22,14 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { const AP_Vehicle::MultiCopter &aparm, AP_MotorsHeli& motors, AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, - AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw + AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw, + RC_Channel& rc_roll, RC_Channel& rc_pitch ) : AC_AttitudeControl(ahrs, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, - pid_rate_roll, pid_rate_pitch, pid_rate_yaw) + pid_rate_roll, pid_rate_pitch, pid_rate_yaw), + _rc_roll(rc_roll), + _rc_pitch(rc_pitch) { AP_Param::setup_object_defaults(this, var_info); } @@ -53,6 +56,10 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { uint8_t leaky_i : 1; // 1 if we should use leaky i term for body-frame rate to motor stage uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots input directly to swash-plate } _flags_heli; + + // references to external libraries + RC_Channel& _rc_roll; + RC_Channel& _rc_pitch; // // body-frame rate controller @@ -69,6 +76,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle virtual int16_t get_angle_boost(int16_t throttle_pwm); + // roll and pitch inputs are sent directly to motor outputs (servos) direct flybar control. + void passthrough_to_motor_roll_pitch(); + // LPF filters to act on Rate Feedforward terms to linearize output. // Due to complicated aerodynamic effects, feedforwards acting too fast can lead // to jerks on rate change requests. From 9326d36e54a06117a7b19f80f7e42d50a81065a4 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 15:52:22 -0400 Subject: [PATCH 015/254] AC_AttitudeControl_Heli: Add use_flybar_passthrough accessor function. --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 4642b804e3e..c9f9a4ea1dc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -41,6 +41,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; } + // use_flybar_passthrough - controls whether we pass-through control inputs to swash-plate + void use_flybar_passthrough(bool passthrough) { _flags_heli.flybar_passthrough = passthrough; } + void update_feedforward_filter_rates(float time_step); // user settable parameters From 184be135cd7cf2390b86359ae267da9fd60f9965 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 16:18:48 -0400 Subject: [PATCH 016/254] TradHeli: Set Flybar passthrough mode in Acro Initialization function. --- ArduCopter/heli_control_acro.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 374cdfcab0d..0710818a89b 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -7,6 +7,9 @@ // heli_acro_init - initialise acro controller static bool heli_acro_init(bool ignore_checks) { + // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos + attitude_control.use_flybar_passthrough(motors.has_flybar()); + // always successfully enter acro return true; } From 1becab3eed5b944bb47e2d4cb74fa329193dd98e Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 17:07:04 -0400 Subject: [PATCH 017/254] TradHeli: Reset flybar passthrough to false when exiting Acro mode. --- ArduCopter/flight_mode.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 35af7ca57a2..9caf7eb316c 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -227,6 +227,13 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); } + +#if FRAME_CONFIG == HELI_FRAME + // firmly reset the flybar passthrough to false when exiting acro mode. + if (old_control_mode == ACRO) { + attitude_control.use_flybar_passthrough(false); + } +#endif //HELI_FRAME } // returns true or false whether mode requires GPS From 32cb2bbce15c1a36c4cfed8429c76e30b9791d49 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 3 Jul 2014 20:55:17 -0400 Subject: [PATCH 018/254] TradHeli: Add yaw-only rate request function for flybar acro mode. --- ArduCopter/heli_control_acro.pde | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 0710818a89b..068569c4b3d 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -34,13 +34,15 @@ static void heli_acro_run() if(motors.armed() && heli_flags.init_targets_on_arming) { heli_flags.init_targets_on_arming=false; attitude_control.relax_bf_rate_controller(); - } - // To-Do: add support for flybarred helis - - // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); + if (!motors.has_flybar()){ + // convert the input to the desired body frame rate + get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); + }else{ + // flybar helis only need yaw rate control + get_pilot_desired_yaw_rate(g.rc_4.control_in, target_yaw); + } // run attitude controller attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); @@ -49,4 +51,15 @@ static void heli_acro_run() attitude_control.set_throttle_out(g.rc_3.control_in, false); } +// get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate +// returns desired yaw rate in centi-degrees-per-second +static void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out) +{ + // calculate rate request + float rate_bf_yaw_request = yaw_in * g.acro_yaw_p; + + // hand back rate request + yaw_out = rate_bf_yaw_request; +} + #endif //HELI_FRAME From 8ac36892ee1b8a6142f3fea2ab2fd112921a495f Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Wed, 9 Jul 2014 15:14:34 -0400 Subject: [PATCH 019/254] Logging: Fix comment error. --- ArduCopter/Log.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index b59e5ada488..1b146eadba9 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -173,7 +173,7 @@ struct PACKED log_AutoTune { float new_gain_sp; // newly calculated gain }; -// Write an Current data packet +// Write an Autotune data packet static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) { struct log_AutoTune pkt = { @@ -195,7 +195,7 @@ struct PACKED log_AutoTuneDetails { float rate_cds; // current rotation rate in centi-degrees / second }; -// Write an Current data packet +// Write an Autotune data packet static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) { struct log_AutoTuneDetails pkt = { From 440f4ebb955ebc980b7944ce55781ca9e0279d32 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Aug 2014 22:14:25 +0900 Subject: [PATCH 020/254] AC_AttControlHeli: add passthrough_bf_roll_pitch_rate_yaw --- .../AC_AttitudeControl_Heli.cpp | 75 ++++++++++++++++--- .../AC_AttitudeControl_Heli.h | 20 +++-- 2 files changed, 72 insertions(+), 23 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 19097129530..1b2f8175572 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -61,6 +61,64 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { AP_GROUPEND }; +// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode +void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf) +{ + // store roll and pitch passthroughs + _passthrough_roll = roll_passthrough; + _passthrough_pitch = pitch_passthrough; + + // set rate controller to use pass through + _flags_heli.flybar_passthrough = true; + + // set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro) + _rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100; + _rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100; + + // accel limit desired yaw rate + if (_accel_y_max > 0.0f) { + float rate_change_limit = _accel_y_max * _dt; + float rate_change = yaw_rate_bf - _rate_bf_desired.z; + rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); + _rate_bf_desired.z += rate_change; + } else { + _rate_bf_desired.z = yaw_rate_bf; + } + + integrate_bf_rate_error_to_angle_errors(); + _angle_bf_error.x = 0; + _angle_bf_error.y = 0; + + // update our earth-frame angle targets + Vector3f angle_ef_error; + frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error); + _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); + _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); + _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + + // handle flipping over pitch axis + if (_angle_ef_target.y > 9000.0f) { + _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); + _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x); + _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); + } + if (_angle_ef_target.y < -9000.0f) { + _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); + _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x); + _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); + } + + // convert body-frame angle errors to body-frame rate targets + update_rate_bf_targets(); + + // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates + _rate_bf_target.x = _rate_bf_desired.x; + _rate_bf_target.y = _rate_bf_desired.y; + + // add desired target to yaw + _rate_bf_target.z += _rate_bf_desired.z; +} + // // rate controller (body-frame) methods // @@ -70,10 +128,11 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { void AC_AttitudeControl_Heli::rate_controller_run() { // call rate controllers and send output to motors object - // To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library? - if (_flags_heli.flybar_passthrough){ - passthrough_to_motor_roll_pitch(); - }else{ + // if using a flybar passthrough roll and pitch directly to motors + if (_flags_heli.flybar_passthrough) { + _motors.set_roll(_passthrough_roll); + _motors.set_pitch(_passthrough_pitch); + } else { rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y); } _motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z)); @@ -245,14 +304,6 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter */ } -// passthrough_to_motor_roll_pitch - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode -void AC_AttitudeControl_Heli::passthrough_to_motor_roll_pitch() -{ - // output to motors - _motors.set_roll(_rc_roll.control_in); - _motors.set_pitch(_rc_pitch.control_in); -} - // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index c9f9a4ea1dc..d9c4db6c851 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -22,18 +22,19 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { const AP_Vehicle::MultiCopter &aparm, AP_MotorsHeli& motors, AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, - AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw, - RC_Channel& rc_roll, RC_Channel& rc_pitch + AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw ) : AC_AttitudeControl(ahrs, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw), - _rc_roll(rc_roll), - _rc_pitch(rc_pitch) + _passthrough_roll(0), _passthrough_pitch(0) { AP_Param::setup_object_defaults(this, var_info); } + // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw + void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf); + // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors // should be called at 100hz or more virtual void rate_controller_run(); @@ -57,12 +58,8 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { uint8_t limit_pitch : 1; // 1 if we have requested larger pitch angle than swash can physically move uint8_t limit_yaw : 1; // 1 if we have requested larger yaw angle than tail servo can physically move uint8_t leaky_i : 1; // 1 if we should use leaky i term for body-frame rate to motor stage - uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots input directly to swash-plate + uint8_t flybar_passthrough : 1; // 1 if we should pass through pilots roll & pitch input directly to swash-plate } _flags_heli; - - // references to external libraries - RC_Channel& _rc_roll; - RC_Channel& _rc_pitch; // // body-frame rate controller @@ -79,8 +76,6 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { // get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle virtual int16_t get_angle_boost(int16_t throttle_pwm); - // roll and pitch inputs are sent directly to motor outputs (servos) direct flybar control. - void passthrough_to_motor_roll_pitch(); // LPF filters to act on Rate Feedforward terms to linearize output. // Due to complicated aerodynamic effects, feedforwards acting too fast can lead @@ -89,6 +84,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { LowPassFilterInt32 roll_feedforward_filter; LowPassFilterInt32 yaw_feedforward_filter; + // pass through for roll and pitch + int16_t _passthrough_roll; + int16_t _passthrough_pitch; }; #endif //AC_ATTITUDECONTROL_HELI_H From c7d6b026b50cdf7d99a88a01625ac1fc4af88cc3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Aug 2014 22:14:48 +0900 Subject: [PATCH 021/254] TradHeli: call passthrough_bf_roll_pitch_rate_yaw --- ArduCopter/heli_control_acro.pde | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 068569c4b3d..4f3feb8d1b6 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -39,14 +39,15 @@ static void heli_acro_run() if (!motors.has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); + // run attitude controller + attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); }else{ // flybar helis only need yaw rate control get_pilot_desired_yaw_rate(g.rc_4.control_in, target_yaw); + // run attitude controller + attitude_control.passthrough_bf_roll_pitch_rate_yaw(g.rc_1.control_in, g.rc_2.control_in, target_yaw); } - // run attitude controller - attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); - // output pilot's throttle without angle boost attitude_control.set_throttle_out(g.rc_3.control_in, false); } From 06880f5cfa33253cb81442483e99274193f4f2fe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Aug 2014 22:16:25 +0900 Subject: [PATCH 022/254] TradHeli: update AttControlHeli constructor reference to rc_1, rc2 are replaced with constant updates during acro_run --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index cb024d8ff50..298fe7880cf 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -648,7 +648,7 @@ static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch, baro_glitch); //////////////////////////////////////////////////////////////////////////////// #if FRAME_CONFIG == HELI_FRAME AC_AttitudeControl_Heli attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, - g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw, g.rc_1, g.rc_2); + g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); #else AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); From f4483203ea99cd47e9572540c20baa722e140884 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Aug 2014 21:11:16 +1000 Subject: [PATCH 023/254] Rover: show firmware version on param list --- APMrover2/GCS_Mavlink.pde | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 26fc5aee566..ebe5073b1f8 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -958,6 +958,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + // mark the firmware version in the tlog + send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING)); + +#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) + send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)); +#endif handle_param_request_list(msg); break; } From f86de61d82bdf83061a1b3fc233d5682b9f77278 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Aug 2014 21:10:59 +1000 Subject: [PATCH 024/254] Copter: show firmware version on param fetch fixes pull #1320 thanks Arthur! --- ArduCopter/GCS_Mavlink.pde | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 798f8ef1489..00b0d36e7f3 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -897,6 +897,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21 { + // mark the firmware version in the tlog + send_text_P(SEVERITY_LOW, PSTR(FIRMWARE_STRING)); + +#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) + send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)); +#endif handle_param_request_list(msg); break; } From 91817b0884d763e3161e1e4c8179a9b2de488892 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Aug 2014 22:50:10 +0900 Subject: [PATCH 025/254] AC_AttControl: div-by-zero check for bf-to-ef conversion --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 18 ++++++++++++------ .../AC_AttitudeControl/AC_AttitudeControl.h | 7 ++++--- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index d4c795f8098..c6cf9e5332b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -382,10 +382,11 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_ } else { _acro_angle_switch = 4500; integrate_bf_rate_error_to_angle_errors(); - frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error); - _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); - _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); - _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) { + _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); + _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); + _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + } if (_angle_ef_target.y > 9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x); @@ -432,12 +433,17 @@ void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Ve } // frame_conversion_bf_to_ef - converts body frame vector to earth frame vector -void AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector) +bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector) { - // convert earth frame rates to body frame rates + // avoid divide by zero + if (_ahrs.cos_pitch() == 0.0f) { + return false; + } + // convert earth frame angle or rates to body frame ef_vector.x = bf_vector.x + _ahrs.sin_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.y + _ahrs.cos_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.z; ef_vector.y = _ahrs.cos_roll() * bf_vector.y - _ahrs.sin_roll() * bf_vector.z; ef_vector.z = (_ahrs.sin_roll() / _ahrs.cos_pitch()) * bf_vector.y + (_ahrs.cos_roll() / _ahrs.cos_pitch()) * bf_vector.z; + return true; } // diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 9b22963f537..2f0c5a11d40 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -116,11 +116,12 @@ class AC_AttitudeControl { // // earth-frame <-> body-frame conversion functions // - // frame_conversion_ef_to_bf - converts earth frame rate targets to body frame rate targets + // frame_conversion_ef_to_bf - converts earth frame angles or rates to body frame void frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f &bf_vector); - // frame_conversion_bf_to_ef - converts body frame rate targets to earth frame rate targets - void frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector); + // frame_conversion_bf_to_ef - converts body frame angles or rates to earth frame + // returns false if conversion fails due to gimbal lock + bool frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector); // // public accessor functions From 06e06438b33c8aa50301994eee23b57d63376b37 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Aug 2014 22:50:33 +0900 Subject: [PATCH 026/254] AC_AttControlHeli: integrate div-by-zero check for bf-to-ef conversion --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 1b2f8175572..9ae1b3ef8fb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -91,10 +91,11 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass // update our earth-frame angle targets Vector3f angle_ef_error; - frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error); - _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); - _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); - _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) { + _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); + _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); + _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); + } // handle flipping over pitch axis if (_angle_ef_target.y > 9000.0f) { From d242fcaae58bd487296172f7da5f3524c7230871 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Aug 2014 22:52:06 +0900 Subject: [PATCH 027/254] Copter: remove get_angle_targets_for_reporting fn this saves a tiny amount of time by removing the memory copy of a Vector3f --- ArduCopter/GCS_Mavlink.pde | 3 +-- ArduCopter/Log.pde | 3 +-- ArduCopter/flight_mode.pde | 5 ----- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 00b0d36e7f3..b1728e46ab8 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -276,8 +276,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) { - Vector3f targets; - get_angle_targets_for_reporting(targets); + const Vector3f &targets = attitude_control.angle_ef_targets(); mavlink_msg_nav_controller_output_send( chan, targets.x / 1.0e2f, diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 1b146eadba9..0f0c8a0fb5d 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -469,8 +469,7 @@ struct PACKED log_Attitude { // Write an attitude packet static void Log_Write_Attitude() { - Vector3f targets; - get_angle_targets_for_reporting(targets); + const Vector3f &targets = attitude_control.angle_ef_targets(); struct log_Attitude pkt = { LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), time_ms : hal.scheduler->millis(), diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 9caf7eb316c..07761919913 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -327,8 +327,3 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) } } -// get_angle_targets_for_reporting() - returns 3d vector of roll, pitch and yaw target angles for logging and reporting to GCS -static void get_angle_targets_for_reporting(Vector3f& targets) -{ - targets = attitude_control.angle_ef_targets(); -} From d7d90b4ff88d2e6745b0f61e7332184a66e5c1de Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Aug 2014 22:57:41 +0900 Subject: [PATCH 028/254] AC_AttControl: remove debug message --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c6cf9e5332b..45a5112c611 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -3,8 +3,6 @@ #include "AC_AttitudeControl.h" #include -extern const AP_HAL::HAL& hal; - // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { @@ -674,7 +672,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) _accel_rp_max = 0.0f; _accel_y_max = 0.0f; } - hal.console->printf_P(PSTR("AccLim:%d"),(int)enable_limits); } // From ec2308bcd23da2b876d16a7ce186461377d9390b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 23 Aug 2014 21:42:23 +0900 Subject: [PATCH 029/254] AC_AttControl: bug fix for ef target during acro --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 45a5112c611..4a2aae82ca9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -387,12 +387,12 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_ } if (_angle_ef_target.y > 9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); - _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x); + _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } if (_angle_ef_target.y < -9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); - _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x); + _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } } From 6a654ff46126f7305e8e176d59e6d9e2a98df10c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 24 Aug 2014 21:00:24 +1000 Subject: [PATCH 030/254] AP_NavEKF: make use_compass() public --- libraries/AP_NavEKF/AP_NavEKF.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index d2b6b5dde70..7f3252da311 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -135,6 +135,10 @@ class NavEKF // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; + // should we use the compass? This is public so it can be used for + // reporting via ahrs.use_compass() + bool use_compass(void) const; + /* return the filter fault status as a bitmasked integer 0 = filter divergence detected via gyro bias growth @@ -510,9 +514,6 @@ class NavEKF perf_counter_t _perf_FuseSideslip; #endif - // should we use the compass? - bool use_compass(void) const; - // should we assume zero sideslip? bool assume_zero_sideslip(void) const; }; From 62d526a50da227a24e2cd9bd64ce5ed105e31f5f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 24 Aug 2014 21:00:56 +1000 Subject: [PATCH 031/254] AP_AHRS: use EKF use_compass() if EKF enabled this allows magfailed status to show on console via SYS_STATUS health bits --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 2098f7b04e5..14b1d0a2769 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -166,6 +166,9 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret) const // true if compass is being used bool AP_AHRS_NavEKF::use_compass(void) { + if (using_EKF()) { + return EKF.use_compass(); + } return AP_AHRS_DCM::use_compass(); } From f78aff67c253d9f6b487f383386ece3dd7d5f328 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Aug 2014 17:25:32 +0900 Subject: [PATCH 032/254] Copter: make landing detector more strict Made more strict by requiring 50 consecutive iterations where the climb rate is below +- 40cm/s. Previously it was 50 cumulative. Removed check of failsafe.radio when clearing the land flag because it could result in the vehicle taking off if the user picked it up. --- ArduCopter/control_land.pde | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index b85e44c84bf..45d29e4f92e 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -188,13 +188,13 @@ static float get_throttle_land() } // update_land_detector - checks if we have landed and updates the ap.land_complete flag -// returns true if we have landed -static bool update_land_detector() +// called at 50hz +static void update_land_detector() { // detect whether we have landed by watching for low climb rate and minimum throttle if (abs(climb_rate) < 40 && motors.limit.throttle_lower) { if (!ap.land_complete) { - // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target) + // increase counter until we hit the trigger then set land complete flag if( land_detector < LAND_DETECTOR_TRIGGER) { land_detector++; }else{ @@ -202,16 +202,14 @@ static bool update_land_detector() land_detector = 0; } } - } else if ((motors.get_throttle_out() > get_non_takeoff_throttle()) || failsafe.radio) { + } else { // we've sensed movement up or down so reset land_detector land_detector = 0; - if(ap.land_complete) { + // if throttle output is high then clear landing flag + if (motors.get_throttle_out() > get_non_takeoff_throttle()) { set_land_complete(false); } } - - // return current state of landing - return ap.land_complete; } // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch From 4038cd2fbd3e27326d0a18191f550ae45886dc35 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 26 Aug 2014 20:50:53 +0900 Subject: [PATCH 033/254] Copter: remote arming check reference to compass learning --- ArduCopter/motors.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index fbdbbf3cfd2..2d90a675718 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -266,7 +266,7 @@ static void pre_arm_checks(bool display_failure) } // check compass learning is on or offsets have been set - if(!compass.learn_offsets_enabled() && !compass.configured()) { + if(!compass.configured()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated")); } From 7802c85b5f43352f3854c368a1bcb76a9dbf3011 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 26 Aug 2014 22:31:43 +0900 Subject: [PATCH 034/254] Copter: pre-arm check of internal vs ext compass --- ArduCopter/config.h | 5 +++++ ArduCopter/motors.pde | 21 +++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index eae4043fc84..b5d446995ce 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -338,6 +338,11 @@ #endif #endif +// arming check's maximum acceptable vector difference between internal and external compass after vectors are normalized to field length of 1.0 +#ifndef COMPASS_ACCEPTABLE_VECTOR_DIFF + #define COMPASS_ACCEPTABLE_VECTOR_DIFF 0.75 // pre arm compass check will fail if internal vs external compass direction differ by more than 45 degrees + #endif + ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2d90a675718..103b548c9b3 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -290,6 +290,27 @@ static void pre_arm_checks(bool display_failure) } return; } + +#if COMPASS_MAX_INSTANCES > 1 + // check all compasses point in roughly same direction + if (compass.get_count() > 1) { + Vector3f prime_mag_vec = compass.get_field(); + prime_mag_vec.normalize(); + for(uint8_t i=0; i COMPASS_ACCEPTABLE_VECTOR_DIFF) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: compasses inconsistent")); + } + return; + } + } + } +#endif + } // check GPS From 5759a6999287bc23e3f667f79d18f462897273d9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Aug 2014 17:04:30 +0900 Subject: [PATCH 035/254] Mission: start next nav cmd immediately after prev completes --- libraries/AP_Mission/AP_Mission.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 69381352649..2c6cb1625d1 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -183,6 +183,12 @@ void AP_Mission::update() if (_cmd_verify_fn(_nav_cmd)) { // market _nav_cmd as complete (it will be started on the next iteration) _flags.nav_cmd_loaded = false; + // immediately advance to the next mission command + if (!advance_current_nav_cmd()) { + // failure to advance nav command means mission has completed + complete(); + return; + } } } From ff94120fbd29969f32d7a816655a98132998a201 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Aug 2014 18:13:59 +0900 Subject: [PATCH 036/254] AC_WPNav: resolve twitch when passing spline waypoints The target positions target velocity was being reset to zero as we passed through a spline waypoint. --- libraries/AC_WPNav/AC_WPNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b7a7b2cfbc5..16f46d23dbe 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -695,7 +695,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination _spline_origin_vel = (_destination - _origin); _spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment? - _spline_vel_scaler = 0.0f; // To-Do: this should be set based on speed at end of prev straight segment? + _spline_vel_scaler = _pos_control.get_vel_target().length(); // start velocity target from current target velocity }else{ // previous segment is splined, vehicle will fly through origin // we can use the previous segment's destination velocity as this segment's origin velocity @@ -707,7 +707,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V }else{ _spline_time = 0.0f; } - _spline_vel_scaler = 0.0f; + // Note: we leave _spline_vel_scaler as it was from end of previous segment } } From 68df4214842dd438e9eb803bd6b6ba0c82413343 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 27 Oct 2013 22:13:42 +0900 Subject: [PATCH 037/254] Copter: throttle deadzone parameter Allows increasing or decreasing the deadband size in AltHold, Loiter, PosHold flight modes --- ArduCopter/Attitude.pde | 8 ++++---- ArduCopter/Parameters.h | 4 +++- ArduCopter/Parameters.pde | 9 +++++++++ ArduCopter/config.h | 4 ++-- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 4daefa0fa0f..3735448fe05 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -151,8 +151,8 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control) // get_pilot_desired_climb_rate - transform pilot's throttle input to // climb rate in cm/s. we use radio_in instead of control_in to get the full range // without any deadzone at the bottom -#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband -#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband +#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+g.throttle_deadzone) // top of the deadband +#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-g.throttle_deadzone) // bottom of the deadband static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) { int16_t desired_rate = 0; @@ -168,10 +168,10 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband - desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone); }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) { // above the deadband - desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone); }else{ // must be in the deadband desired_rate = 0; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 72c68991428..b1fe5e4df76 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -119,7 +119,8 @@ class Parameters { k_param_sonar, // sonar object k_param_ekfcheck_thresh, k_param_terrain, - k_param_acro_expo, // 56 + k_param_acro_expo, + k_param_throttle_deadzone, // 57 // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -365,6 +366,7 @@ class Parameters { AP_Int16 failsafe_throttle_value; AP_Int16 throttle_cruise; AP_Int16 throttle_mid; + AP_Int16 throttle_deadzone; // Flight modes // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 96522ae61ae..09a0e99d645 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -295,6 +295,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 1 GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT), + // @Param: THR_DZ + // @DisplayName: Throttle deadzone + // @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes + // @User: Standard + // @Range: 0 300 + // @Units: pwm + // @Increment: 1 + GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT), + // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b5d446995ce..f5910a3a695 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -659,8 +659,8 @@ # define THR_MAX_DEFAULT 1000 // maximum throttle sent to the motors #endif -#ifndef THROTTLE_IN_DEADBAND -# define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM +#ifndef THR_DZ_DEFAULT +# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter #endif #ifndef ALT_HOLD_P From 515c0d863017398460da8ceec331e8de144d1e45 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 29 Aug 2014 14:00:29 -0700 Subject: [PATCH 038/254] Copter: restore CLI set parameter feature Added back the ability to set params from CLI --- ArduCopter/setup.pde | 61 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 994d2368f87..c1bc2c67a52 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -9,6 +9,7 @@ // Functions called from the setup menu static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); +static int8_t setup_set (uint8_t argc, const Menu::arg *argv); #ifdef WITH_ESC_CALIB static int8_t esc_calib (uint8_t argc, const Menu::arg *argv); #endif @@ -19,6 +20,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { // ======= =============== {"reset", setup_factory}, {"show", setup_show}, + {"set", setup_set}, #ifdef WITH_ESC_CALIB {"esc_calib", esc_calib}, #endif @@ -66,6 +68,65 @@ setup_factory(uint8_t argc, const Menu::arg *argv) return(0); } +//Set a parameter to a specified value. It will cast the value to the current type of the +//parameter and make sure it fits in case of INT8 and INT16 +static int8_t setup_set(uint8_t argc, const Menu::arg *argv) +{ + int8_t value_int8; + int16_t value_int16; + + AP_Param *param; + enum ap_var_type p_type; + + if(argc!=3) + { + cliSerial->printf_P(PSTR("Invalid command. Usage: set \n")); + return 0; + } + + param = AP_Param::find(argv[1].str, &p_type); + if(!param) + { + cliSerial->printf_P(PSTR("Param not found: %s\n"), argv[1].str); + return 0; + } + + switch(p_type) + { + case AP_PARAM_INT8: + value_int8 = (int8_t)(argv[2].i); + if(argv[2].i!=value_int8) + { + cliSerial->printf_P(PSTR("Value out of range for type INT8\n")); + return 0; + } + ((AP_Int8*)param)->set_and_save(value_int8); + break; + case AP_PARAM_INT16: + value_int16 = (int16_t)(argv[2].i); + if(argv[2].i!=value_int16) + { + cliSerial->printf_P(PSTR("Value out of range for type INT16\n")); + return 0; + } + ((AP_Int16*)param)->set_and_save(value_int16); + break; + + //int32 and float don't need bounds checking, just use the value provoded by Menu::arg + case AP_PARAM_INT32: + ((AP_Int32*)param)->set_and_save(argv[2].i); + break; + case AP_PARAM_FLOAT: + ((AP_Float*)param)->set_and_save(argv[2].f); + break; + default: + cliSerial->printf_P(PSTR("Cannot set parameter of type %d.\n"), p_type); + break; + } + + return 0; +} + // Print the current configuration. // Called by the setup menu 'show' command. static int8_t From 15508c49ef41ec0864698ca048978e8ff88fa61f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 31 Aug 2014 10:44:09 +0900 Subject: [PATCH 039/254] Copter: default LAND_REPOSITION to 1 --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index f5910a3a695..af12ca5c985 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -451,7 +451,7 @@ # define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED #endif #ifndef LAND_REPOSITION_DEFAULT - # define LAND_REPOSITION_DEFAULT 0 // by default the pilot cannot override roll/pitch during landing + # define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing #endif #ifndef LAND_WITH_DELAY_MS # define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event From 5d7a2726e2f3b0888d05239f7c04a4b581f48f36 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Thu, 21 Aug 2014 17:35:41 +0200 Subject: [PATCH 040/254] AP_HAL_VRBRAIN: added management for external relay 1 and 2 --- libraries/AP_HAL_VRBRAIN/GPIO.cpp | 50 ++++++++++++++++++++++++++----- libraries/AP_HAL_VRBRAIN/GPIO.h | 2 ++ 2 files changed, 45 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.cpp b/libraries/AP_HAL_VRBRAIN/GPIO.cpp index 0eb081ebd6c..a6fa2243fd1 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.cpp +++ b/libraries/AP_HAL_VRBRAIN/GPIO.cpp @@ -44,22 +44,23 @@ void VRBRAINGPIO::init() if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) { hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n"); } -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT1) if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) { hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n"); } #endif -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT2) if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) { hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n"); } #endif -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) +#if defined(LED_EXT3) if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) { hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n"); } #endif +#if defined(BUZZER_EXT) _buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR); if (_buzzer_fd == -1) { hal.scheduler->panic("Unable to open " BUZZER_DEVICE_PATH); @@ -67,6 +68,15 @@ void VRBRAINGPIO::init() if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) { hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n"); } +#endif + +#if defined(GPIO_GPIO0_OUTPUT) + stm32_configgpio(GPIO_GPIO0_OUTPUT); +#endif + +#if defined(GPIO_GPIO1_OUTPUT) + stm32_configgpio(GPIO_GPIO1_OUTPUT); +#endif } void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output) @@ -85,6 +95,16 @@ uint8_t VRBRAINGPIO::read(uint8_t pin) { switch (pin) { + case EXTERNAL_RELAY1_PIN: +#if defined(GPIO_GPIO0_OUTPUT) + return (stm32_gpioread(GPIO_GPIO0_OUTPUT))?HIGH:LOW; +#endif + break; + case EXTERNAL_RELAY2_PIN: +#if defined(GPIO_GPIO1_OUTPUT) + return (stm32_gpioread(GPIO_GPIO1_OUTPUT))?HIGH:LOW; +#endif + break; } return LOW; } @@ -118,7 +138,7 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value) break; case EXTERNAL_LED_GPS: -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT1) if (value == LOW) { ioctl(_led_fd, LED_OFF, LED_EXT1); } else { @@ -128,7 +148,7 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value) break; case EXTERNAL_LED_ARMED: -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT2) if (value == LOW) { ioctl(_led_fd, LED_OFF, LED_EXT2); } else { @@ -144,12 +164,26 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value) break; case BUZZER_PIN: +#if defined(BUZZER_EXT) if (value == LOW) { ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT); } else { ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT); } +#endif break; + + case EXTERNAL_RELAY1_PIN: +#if defined(GPIO_GPIO0_OUTPUT) + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, (value==HIGH)); +#endif + break; + case EXTERNAL_RELAY2_PIN: +#if defined(GPIO_GPIO1_OUTPUT) + stm32_gpiowrite(GPIO_GPIO1_OUTPUT, (value==HIGH)); +#endif + break; + } } @@ -170,13 +204,13 @@ void VRBRAINGPIO::toggle(uint8_t pin) break; case EXTERNAL_LED_GPS: -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT1) ioctl(_led_fd, LED_TOGGLE, LED_EXT1); #endif break; case EXTERNAL_LED_ARMED: -#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) +#if defined(LED_EXT2) ioctl(_led_fd, LED_TOGGLE, LED_EXT2); #endif break; @@ -190,7 +224,9 @@ void VRBRAINGPIO::toggle(uint8_t pin) break; case BUZZER_PIN: +#if defined(BUZZER_EXT) ioctl(_buzzer_fd, BUZZER_TOGGLE, BUZZER_EXT); +#endif break; default: diff --git a/libraries/AP_HAL_VRBRAIN/GPIO.h b/libraries/AP_HAL_VRBRAIN/GPIO.h index 2d75ffa1ad1..6dd7a5a113f 100644 --- a/libraries/AP_HAL_VRBRAIN/GPIO.h +++ b/libraries/AP_HAL_VRBRAIN/GPIO.h @@ -13,6 +13,8 @@ # define EXTERNAL_LED_MOTOR1 30 # define EXTERNAL_LED_MOTOR2 31 # define BUZZER_PIN 32 + # define EXTERNAL_RELAY1_PIN 33 + # define EXTERNAL_RELAY2_PIN 34 # define HAL_GPIO_LED_ON HIGH # define HAL_GPIO_LED_OFF LOW From 781f15ba596b06b92e5837c0c89ed32c5c42376a Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Thu, 21 Aug 2014 17:34:38 +0200 Subject: [PATCH 041/254] AP_relay: added default relay pin for VRBRAIN --- libraries/AP_Relay/AP_Relay.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index dcbd6ec9b98..75fe1e175cf 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -21,7 +21,7 @@ #define RELAY_PIN 54 #endif #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#define RELAY_PIN -1 +#define RELAY_PIN 33 #else // no relay for this board #define RELAY_PIN -1 From f6fada2d49c888715ff2ea367c6daa8446c500f3 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Tue, 19 Aug 2014 08:29:32 +0200 Subject: [PATCH 042/254] VRBRAIN: added startup of internal mag's driver for VR Brain Standard 5 --- mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM | 15 +++++++++++++++ mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS | 15 +++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM index a3d7f5cfe67..06bb238879b 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM @@ -94,6 +94,21 @@ else # sh /etc/init.d/rc.error fi +echo "[APM] Starting HMC5883 Internal" +if hmc5883 -I start +then + echo "[APM] HMC5883 Internal started OK" + if hmc5883 -I calibrate + then + echo "[APM] HMC5883 Internal calibrate OK" + else + echo "[APM] HMC5883 Internal calibrate failed" + fi +else + echo "[APM] HMC5883 Internal start failed" +# sh /etc/init.d/rc.error +fi + echo "[APM] Starting HMC5883 External GPS" if hmc5883 -X start then diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS index 141877a08b2..9a532444aa0 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS @@ -67,6 +67,21 @@ else # sh /etc/init.d/rc.error fi +echo "[APMNS] Starting HMC5883 Internal" +if hmc5883 -I start +then + echo "[APMNS] HMC5883 Internal started OK" + if hmc5883 -I calibrate + then + echo "[APMNS] HMC5883 Internal calibrate OK" + else + echo "[APMNS] HMC5883 Internal calibrate failed" + fi +else + echo "[APMNS] HMC5883 Internal start failed" +# sh /etc/init.d/rc.error +fi + echo "[APMNS] Starting HMC5883 External GPS" if hmc5883 -X start then From ada7be6ae153052fd2790e51b004d6ae958cdea6 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Fri, 22 Aug 2014 18:36:11 +0200 Subject: [PATCH 043/254] AP_HAL: VRBRAIN corrected EEPROM size and added terrain folder on MicroSD --- libraries/AP_HAL/AP_HAL_Boards.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 401b2b0a9da..ab4710f6fd4 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -215,9 +215,10 @@ #define HAL_BOARD_NAME "VRBRAIN" #define HAL_CPU_CLASS HAL_CPU_CLASS_150 #define HAL_OS_POSIX_IO 1 -#define HAL_STORAGE_SIZE 4096 +#define HAL_STORAGE_SIZE 8192 #define HAL_STORAGE_SIZE_AVAILABLE HAL_STORAGE_SIZE #define HAL_BOARD_LOG_DIRECTORY "/fs/microsd/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/fs/microsd/APM/TERRAIN" #define HAL_INS_DEFAULT HAL_INS_VRBRAIN #define HAL_BARO_DEFAULT HAL_BARO_VRBRAIN #define HAL_COMPASS_DEFAULT HAL_COMPASS_VRBRAIN From e995641e48e013944e05567967b0efa287481b92 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Fri, 22 Aug 2014 18:19:37 +0200 Subject: [PATCH 044/254] AP_HAL_VRBRAIN: enable 2nd GPS for VRBRAIN 5 --- libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp index 175685ea77b..ad17dd93fbe 100644 --- a/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp +++ b/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp @@ -67,7 +67,7 @@ static VRBRAINGPIO gpioDriver; #define UARTB_DEFAULT_DEVICE "/dev/ttyS0" #define UARTC_DEFAULT_DEVICE "/dev/ttyS2" #define UARTD_DEFAULT_DEVICE "/dev/null" -#define UARTE_DEFAULT_DEVICE "/dev/null" +#define UARTE_DEFAULT_DEVICE "/dev/ttyS1" #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" #define UARTB_DEFAULT_DEVICE "/dev/ttyS0" From 4fa8f82c59e560ae3fbaa9d8894753617134ab25 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Sun, 24 Aug 2014 12:39:53 +0200 Subject: [PATCH 045/254] Build: VRBRAIN corrected order of MAG startup --- mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM | 28 +++++++++---------- .../ROMFS_VRBRAIN51_APM/init.d/rc.APMNS | 28 +++++++++---------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM index 06bb238879b..b3e853914ec 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM @@ -94,33 +94,33 @@ else # sh /etc/init.d/rc.error fi -echo "[APM] Starting HMC5883 Internal" -if hmc5883 -I start +echo "[APM] Starting HMC5883 External GPS" +if hmc5883 -X start then - echo "[APM] HMC5883 Internal started OK" - if hmc5883 -I calibrate + echo "[APM] HMC5883 External GPS started OK" + if hmc5883 -X calibrate then - echo "[APM] HMC5883 Internal calibrate OK" + echo "[APM] HMC5883 External GPS calibrate OK" else - echo "[APM] HMC5883 Internal calibrate failed" + echo "[APM] HMC5883 External GPS calibrate failed" fi else - echo "[APM] HMC5883 Internal start failed" + echo "[APM] HMC5883 External GPS start failed" # sh /etc/init.d/rc.error fi -echo "[APM] Starting HMC5883 External GPS" -if hmc5883 -X start +echo "[APM] Starting HMC5883 Internal" +if hmc5883 -I start then - echo "[APM] HMC5883 External GPS started OK" - if hmc5883 -X calibrate + echo "[APM] HMC5883 Internal started OK" + if hmc5883 -I calibrate then - echo "[APM] HMC5883 External GPS calibrate OK" + echo "[APM] HMC5883 Internal calibrate OK" else - echo "[APM] HMC5883 External GPS calibrate failed" + echo "[APM] HMC5883 Internal calibrate failed" fi else - echo "[APM] HMC5883 External GPS start failed" + echo "[APM] HMC5883 Internal start failed" # sh /etc/init.d/rc.error fi diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS index 9a532444aa0..25fc31fd29d 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS @@ -67,33 +67,33 @@ else # sh /etc/init.d/rc.error fi -echo "[APMNS] Starting HMC5883 Internal" -if hmc5883 -I start +echo "[APMNS] Starting HMC5883 External GPS" +if hmc5883 -X start then - echo "[APMNS] HMC5883 Internal started OK" - if hmc5883 -I calibrate + echo "[APMNS] HMC5883 External GPS started OK" + if hmc5883 -X calibrate then - echo "[APMNS] HMC5883 Internal calibrate OK" + echo "[APMNS] HMC5883 External GPS calibrate OK" else - echo "[APMNS] HMC5883 Internal calibrate failed" + echo "[APMNS] HMC5883 External GPS calibrate failed" fi else - echo "[APMNS] HMC5883 Internal start failed" + echo "[APMNS] HMC5883 External GPS start failed" # sh /etc/init.d/rc.error fi -echo "[APMNS] Starting HMC5883 External GPS" -if hmc5883 -X start +echo "[APMNS] Starting HMC5883 Internal" +if hmc5883 -I start then - echo "[APMNS] HMC5883 External GPS started OK" - if hmc5883 -X calibrate + echo "[APMNS] HMC5883 Internal started OK" + if hmc5883 -I calibrate then - echo "[APMNS] HMC5883 External GPS calibrate OK" + echo "[APMNS] HMC5883 Internal calibrate OK" else - echo "[APMNS] HMC5883 External GPS calibrate failed" + echo "[APMNS] HMC5883 Internal calibrate failed" fi else - echo "[APMNS] HMC5883 External GPS start failed" + echo "[APMNS] HMC5883 Internal start failed" # sh /etc/init.d/rc.error fi From 938f6f2c479093d6c9bfafed721c721f678094c8 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Sun, 24 Aug 2014 13:55:31 +0200 Subject: [PATCH 046/254] Build: VRBRAIN corrected order of MAG startup for 4.5 board --- mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APM | 29 ++++++++++--------- .../ROMFS_VRBRAIN45_APM/init.d/rc.APMNS | 28 +++++++++--------- 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APM b/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APM index 9dbbfbde82a..0643f63aa85 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APM +++ b/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APM @@ -76,20 +76,7 @@ else sh /etc/init.d/rc.error fi -echo "[APM] Starting HMC5883 Internal" -if hmc5883 -I start -then - echo "[APM] HMC5883 onboard started OK" - if hmc5883 calibrate - then - echo "[APM] HMC5883 onboard calibrate OK" - else - echo "[APM] HMC5883 onboard calibrate failed" - fi -else - echo "[APM] HMC5883 onboard start failed" -# sh /etc/init.d/rc.error -fi + echo "[APM] Starting HMC5883 External GPS" if hmc5883 -X start @@ -106,6 +93,20 @@ else # sh /etc/init.d/rc.error fi +echo "[APM] Starting HMC5883 Internal" +if hmc5883 -I start +then + echo "[APM] HMC5883 onboard started OK" + if hmc5883 -I calibrate + then + echo "[APM] HMC5883 onboard calibrate OK" + else + echo "[APM] HMC5883 onboard calibrate failed" + fi +else + echo "[APM] HMC5883 onboard start failed" +# sh /etc/init.d/rc.error +fi echo "[APM] Starting MPU6000 Internal" if mpu6000 -I start then diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APMNS b/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APMNS index 9989eeac76b..716842f92a9 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APMNS +++ b/mk/VRBRAIN/ROMFS_VRBRAIN45_APM/init.d/rc.APMNS @@ -49,20 +49,6 @@ else sh /etc/init.d/rc.error fi -echo "[APMNS] Starting HMC5883 Internal" -if hmc5883 -I start -then - echo "[APMNS] HMC5883 onboard started OK" - if hmc5883 calibrate - then - echo "[APMNS] HMC5883 onboard calibrate OK" - else - echo "[APMNS] HMC5883 onboard calibrate failed" - fi -else - echo "[APMNS] HMC5883 onboard start failed" -# sh /etc/init.d/rc.error -fi echo "[APMNS] Starting HMC5883 External GPS" if hmc5883 -X start @@ -79,6 +65,20 @@ else # sh /etc/init.d/rc.error fi +echo "[APMNS] Starting HMC5883 Internal" +if hmc5883 -I start +then + echo "[APMNS] HMC5883 onboard started OK" + if hmc5883 -I calibrate + then + echo "[APMNS] HMC5883 onboard calibrate OK" + else + echo "[APMNS] HMC5883 onboard calibrate failed" + fi +else + echo "[APMNS] HMC5883 onboard start failed" +# sh /etc/init.d/rc.error +fi echo "[APMNS] Starting MPU6000 Internal" if mpu6000 -I start then From be2dabe58ea5a86653fb6408b038f76879270ba3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 31 Aug 2014 11:55:06 +0900 Subject: [PATCH 047/254] Copter: AC3.2-rc6 release notes --- ArduCopter/ReleaseNotes.txt | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 879b888bd6d..8ebff8efc21 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,18 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc6 31-Aug-2014 +Changes from 3.2-rc5 +1) Spline twitch when passing through a waypoint largely resolved +2) THR_DZ param added to allow user configuration of throttle deadzone during AltHold, Loiter, PosHold +3) Landing check made more strict (climb rate must be -40~40cm/s for 1 full second) +4) LAND_REPOSITION param default set to 1 +5) TradHeli with flybar passes through pilot inputs directly to swash when in ACRO mode +6) Safety Items: + a) EKF check disabled when using inertial nav (caused too many false positives) + b) pre-arm check of internal vs external compass direction (must be within 45deg of each other) +7) Bug Fixes: + a) bug fix to resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode +------------------------------------------------------------------ ArduCopter 3.2-rc5 15-Aug-2014 Changes from 3.2-rc4 1) Pixhawk's max num waypoints increased to 718 From acecc784540afee9dac7101928c38abe6a7d83c7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 31 Aug 2014 11:56:58 +0900 Subject: [PATCH 048/254] Copter: version to AC3.2-rc6 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 298fe7880cf..76b60cfddc6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc5" +#define THISFIRMWARE "ArduCopter V3.2-rc6" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 7236d0621abd977e0ef9a2c00758198198419dab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 31 Aug 2014 12:02:00 +0900 Subject: [PATCH 049/254] Copter: update AC3.2-rc6 release notes Forgot to add GPS driver buffer overflow item --- ArduCopter/ReleaseNotes.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 8ebff8efc21..a230e8ddd64 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -11,7 +11,8 @@ Changes from 3.2-rc5 a) EKF check disabled when using inertial nav (caused too many false positives) b) pre-arm check of internal vs external compass direction (must be within 45deg of each other) 7) Bug Fixes: - a) bug fix to resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode + a) resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode + b) resolve GPS driver buffer overflow that could lead to missed GPS messages on Pixhawk/PX4 boards ------------------------------------------------------------------ ArduCopter 3.2-rc5 15-Aug-2014 Changes from 3.2-rc4 From 162d421e5df550cb95a3681d28f4ec0bdf4067eb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 31 Aug 2014 12:05:33 +0900 Subject: [PATCH 050/254] Copter: update AC3.2-rc6 release notes forgot compass-not-calibrated bug fix --- ArduCopter/ReleaseNotes.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index a230e8ddd64..37d21dbdb92 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -12,7 +12,8 @@ Changes from 3.2-rc5 b) pre-arm check of internal vs external compass direction (must be within 45deg of each other) 7) Bug Fixes: a) resolve NaN in angle targets when vehicle hits gimbal lock in ACRO mode - b) resolve GPS driver buffer overflow that could lead to missed GPS messages on Pixhawk/PX4 boards + b) resolve GPS driver buffer overflow that could lead to missed GPS messages on Pixhawk/PX4 boards + c) resolve false "compass not calibrated" warnings on Pixhawk/PX4 caused by missing device id initialisation ------------------------------------------------------------------ ArduCopter 3.2-rc5 15-Aug-2014 Changes from 3.2-rc4 From 0f2083a9b8a58ddecfcfc4c7efe45b255039a910 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Thu, 14 Aug 2014 08:54:00 +0200 Subject: [PATCH 051/254] AP_HAL: added missing CONFIG_HAL_BOARD_SUBTYPE #define for HAL_BOARD_VRBRAIN --- libraries/AP_HAL/AP_HAL_Boards.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index ab4710f6fd4..cea2d197ced 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -223,6 +223,7 @@ #define HAL_BARO_DEFAULT HAL_BARO_VRBRAIN #define HAL_COMPASS_DEFAULT HAL_COMPASS_VRBRAIN #define HAL_SERIAL0_BAUD_DEFAULT 115200 +#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_NONE #else #error "Unknown CONFIG_HAL_BOARD type" From 1ee6481517cb5e8adfdc9b6a3f88a6c5bf6fa522 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 2 Sep 2014 12:38:58 +0900 Subject: [PATCH 052/254] Copter: sanity check throttle deadzone --- ArduCopter/Attitude.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3735448fe05..0137a3c6f32 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -165,6 +165,9 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) // ensure a reasonable throttle value throttle_control = constrain_int16(throttle_control,0,1000); + // ensure a reasonable deadzone + g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400); + // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband From 5ead80994ec58c3b50b8c2ac20428db823f760b7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Sep 2014 15:28:07 +0900 Subject: [PATCH 053/254] InertialSensor: reorder .cpp file to match .h No functional changes --- .../AP_InertialSensor/AP_InertialSensor.cpp | 422 +++++++++--------- 1 file changed, 211 insertions(+), 211 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index d1bbac8333c..3cb39d5b358 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -131,130 +131,164 @@ AP_InertialSensor::init( Start_style style, } } -// save parameters to eeprom -void AP_InertialSensor::_save_parameters() -{ - _product_id.save(); - for (uint8_t i=0; iprint_P(PSTR("Init Gyro")); - - // flash leds to tell user to keep the IMU still - AP_Notify::flags.initialising = true; + uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES); + Vector3f samples[INS_MAX_INSTANCES][6]; + Vector3f new_offsets[INS_MAX_INSTANCES]; + Vector3f new_scaling[INS_MAX_INSTANCES]; + Vector3f orig_offset[INS_MAX_INSTANCES]; + Vector3f orig_scale[INS_MAX_INSTANCES]; + uint8_t num_ok = 0; - // remove existing gyro offsets - for (uint8_t k=0; kdelay(5); - update(); + // clear accelerometer offsets and scaling + _accel_offset[k] = Vector3f(0,0,0); + _accel_scale[k] = Vector3f(1,1,1); } - // the strategy is to average 50 points over 0.5 seconds, then do it - // again and see if the 2nd average is within a small margin of - // the first - - uint8_t num_converged = 0; + // capture data from 6 positions + for (uint8_t i=0; i<6; i++) { + const prog_char_t *msg; - // we try to get a good calibration estimate for up to 10 seconds - // if the gyros are stable, we should get it in 1 second - for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) { - Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES]; - float diff_norm[INS_MAX_INSTANCES]; - uint8_t i; + // display message to user + switch ( i ) { + case 0: + msg = PSTR("level"); + break; + case 1: + msg = PSTR("on its LEFT side"); + break; + case 2: + msg = PSTR("on its RIGHT side"); + break; + case 3: + msg = PSTR("nose DOWN"); + break; + case 4: + msg = PSTR("nose UP"); + break; + default: // default added to avoid compiler warning + case 5: + msg = PSTR("on its BACK"); + break; + } + interact->printf_P( + PSTR("Place vehicle %S and press any key.\n"), msg); - memset(diff_norm, 0, sizeof(diff_norm)); + // wait for user input + if (!interact->blocking_read()) { + //No need to use interact->printf_P for an error, blocking_read does this when it fails + goto failed; + } - hal.console->print_P(PSTR("*")); + // clear out any existing samples from ins + update(); - for (uint8_t k=0; kprintf_P(PSTR("Failed to get INS sample\n")); + goto failed; + } + // read samples from ins update(); - for (uint8_t k=0; kdelay(5); - } - for (uint8_t k=0; kdelay(10); + num_samples++; } - - for (uint8_t k=0; kprintf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"), + (unsigned)k, + new_offsets[k].x, new_offsets[k].y, new_offsets[k].z); + interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"), + (unsigned)k, + new_scaling[k].x, new_scaling[k].y, new_scaling[k].z); + if (success) num_ok++; } - // we've kept the user waiting long enough - use the best pair we - // found so far - hal.console->println(); - for (uint8_t k=0; kprintf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), - (unsigned)k, ToDeg(best_diff[k])); - _gyro_offset[k] = best_avg[k]; + if (num_ok == num_accels) { + interact->printf_P(PSTR("Calibration successful\n")); + + for (uint8_t k=0; kprintf_P(PSTR("Calibration FAILED\n")); + // restore original scaling and offsets + for (uint8_t k=0; kprint_P(PSTR("Init Gyro")); - // clear accelerometer offsets and scaling - _accel_offset[k] = Vector3f(0,0,0); - _accel_scale[k] = Vector3f(1,1,1); + // flash leds to tell user to keep the IMU still + AP_Notify::flags.initialising = true; + + // remove existing gyro offsets + for (uint8_t k=0; kdelay(5); + update(); + } - // display message to user - switch ( i ) { - case 0: - msg = PSTR("level"); - break; - case 1: - msg = PSTR("on its LEFT side"); - break; - case 2: - msg = PSTR("on its RIGHT side"); - break; - case 3: - msg = PSTR("nose DOWN"); - break; - case 4: - msg = PSTR("nose UP"); - break; - default: // default added to avoid compiler warning - case 5: - msg = PSTR("on its BACK"); - break; - } - interact->printf_P( - PSTR("Place vehicle %S and press any key.\n"), msg); + // the strategy is to average 50 points over 0.5 seconds, then do it + // again and see if the 2nd average is within a small margin of + // the first - // wait for user input - if (!interact->blocking_read()) { - //No need to use interact->printf_P for an error, blocking_read does this when it fails - goto failed; - } + uint8_t num_converged = 0; - // clear out any existing samples from ins - update(); + // we try to get a good calibration estimate for up to 10 seconds + // if the gyros are stable, we should get it in 1 second + for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) { + Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES]; + float diff_norm[INS_MAX_INSTANCES]; + uint8_t i; - // average 32 samples - for (uint8_t k=0; kprint_P(PSTR("*")); + + for (uint8_t k=0; kprintf_P(PSTR("Failed to get INS sample\n")); - goto failed; - } - // read samples from ins + for (i=0; i<50; i++) { update(); - // capture sample - for (uint8_t k=0; kdelay(10); - num_samples++; + hal.scheduler->delay(5); } - for (uint8_t k=0; kprintf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"), - (unsigned)k, - new_offsets[k].x, new_offsets[k].y, new_offsets[k].z); - interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"), - (unsigned)k, - new_scaling[k].x, new_scaling[k].y, new_scaling[k].z); - if (success) num_ok++; - } - - if (num_ok == num_accels) { - interact->printf_P(PSTR("Calibration successful\n")); - for (uint8_t k=0; kprintf_P(PSTR("Calibration FAILED\n")); - // restore original scaling and offsets - for (uint8_t k=0; kprintln(); + for (uint8_t k=0; kprintf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), + (unsigned)k, ToDeg(best_diff[k])); + _gyro_offset[k] = best_avg[k]; } } - // if we got this far the accelerometers must have been calibrated - return true; } +#if !defined( __AVR_ATmega1280__ ) // _calibrate_model - perform low level accel calibration // accel_sample are accelerometer samples collected in 6 different positions // accel_offsets are output from the calibration routine @@ -679,3 +669,13 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, #endif // __AVR_ATmega1280__ +// save parameters to eeprom +void AP_InertialSensor::_save_parameters() +{ + _product_id.save(); + for (uint8_t i=0; i Date: Mon, 1 Sep 2014 20:20:27 +0900 Subject: [PATCH 054/254] INS: add get_accel_health_all and get_gyro_health_all Returns true only if all available accels or gyros are healthy --- .../AP_InertialSensor/AP_InertialSensor.cpp | 24 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 2 ++ 2 files changed, 26 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 3cb39d5b358..1f99d3079ad 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -294,6 +294,30 @@ AP_InertialSensor::init_gyro() _save_parameters(); } +// get_gyro_health_all - return true if all gyros are healthy +bool AP_InertialSensor::get_gyro_health_all(void) const +{ + for (uint8_t i=0; i 0); +} + +// get_accel_health_all - return true if all accels are healthy +bool AP_InertialSensor::get_accel_health_all(void) const +{ + for (uint8_t i=0; i 0); +} + void AP_InertialSensor::_init_accel() { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 78f13baafce..70611c94d93 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -118,10 +118,12 @@ class AP_InertialSensor // multi-device interface virtual bool get_gyro_health(uint8_t instance) const { return true; } bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } + bool get_gyro_health_all(void) const; virtual uint8_t get_gyro_count(void) const { return 1; }; virtual bool get_accel_health(uint8_t instance) const { return true; } bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } + bool get_accel_health_all(void) const; virtual uint8_t get_accel_count(void) const { return 1; }; // get accel offsets in m/s/s From 6c0b9a2cfc8f914f4177d5353ee5f781b32f98bd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Sep 2014 20:21:19 +0900 Subject: [PATCH 055/254] Copter: check all gyros and accels in pre-arm check --- ArduCopter/motors.pde | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 103b548c9b3..591b8ceb13b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -338,10 +338,18 @@ static void pre_arm_checks(bool display_failure) return; } - // check accels and gyros are healthy - if(!ins.healthy()) { + // check accels are healthy + if(!ins.get_accel_health_all()) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels not healthy")); + } + return; + } + + // check gyros are healthy + if(!ins.get_gyro_health_all()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros not healthy")); } return; } From 8b91900b74c0ae91e1882884f4a18c7897e9c813 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Sep 2014 20:22:18 +0900 Subject: [PATCH 056/254] Copter: individual accel and gyro status to GCS --- ArduCopter/GCS_Mavlink.pde | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b1728e46ab8..8de62a6c134 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -199,8 +199,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (ap.rc_receiver_present && !failsafe.radio) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } - if (!ins.healthy()) { - control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL); + if (!ins.get_gyro_health_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + if (!ins.get_accel_health_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; } if (!ahrs.healthy()) { From 8e7b93d1b764fba04e71b653614e70465f1ab74b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Sep 2014 13:50:41 +0900 Subject: [PATCH 057/254] Copter: pre-arm consistency check of accels --- ArduCopter/config.h | 5 +++++ ArduCopter/motors.pde | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index af12ca5c985..11c97eaa86c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -304,6 +304,11 @@ # define PREARM_MAX_VELOCITY_CMS 50.0f // vehicle must be travelling under 50cm/s before arming #endif +// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers +#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF + #define PREARM_MAX_ACCEL_VECTOR_DIFF 1.0f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s +#endif + ////////////////////////////////////////////////////////////////////////////// // EKF Checker #ifndef EKFCHECK_THRESHOLD_DEFAULT diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 591b8ceb13b..f0feadafdf8 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -346,6 +346,24 @@ static void pre_arm_checks(bool display_failure) return; } +#if INS_MAX_INSTANCES > 1 + // check all accelerometers point in roughly same direction + if (ins.get_accel_count() > 1) { + const Vector3f &prime_accel_vec = ins.get_accel(); + for(uint8_t i=0; i PREARM_MAX_ACCEL_VECTOR_DIFF) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels inconsistent")); + } + return; + } + } + } +#endif + // check gyros are healthy if(!ins.get_gyro_health_all()) { if (display_failure) { From b214b8ba1561cfbab2c89f2c07928919b612234f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Sep 2014 12:37:26 +0900 Subject: [PATCH 058/254] Copter: add short delay to arming to allow RC input The short delay gives time for the RC inputs to be processed which removes the chance of a false-positive on the "late frame" radio check. A false positive could lead to an immediate disarm right after arming. --- ArduCopter/motors.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index f0feadafdf8..f6c63442f7e 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -197,6 +197,9 @@ static void init_arm_motors() sprayer.test_pump(false); #endif + // short delay to allow reading of rc inputs + delay(30); + // enable output to motors output_min(); From dbb0283dbab28d730c7eee25ac1401308c4af391 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Sep 2014 20:43:51 +0900 Subject: [PATCH 059/254] Copter: land check gets overall throttle and rotation rate check add check that overall throttle level is below the non-takeoff throttle instead of just checking that it's motors have hit their lower limits because low limits can also be caused by high yaw rotation requests. Absolute climb rate requirement reduced to 30cm/s --- ArduCopter/config.h | 6 ++++++ ArduCopter/control_land.pde | 5 +++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 11c97eaa86c..33c781735dc 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -452,6 +452,12 @@ #ifndef LAND_DETECTOR_TRIGGER # define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. #endif +#ifndef LAND_DETECTOR_CLIMBRATE_MAX +# define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s +#endif +#ifndef LAND_DETECTOR_ROTATION_MAX + # define LAND_DETECTOR_ROTATION_MAX 0.50f // vehicle rotation must be below 0.5 rad/sec (=30deg/sec for) vehicle to consider itself landed +#endif #ifndef LAND_REQUIRE_MIN_THROTTLE_TO_DISARM // require pilot to reduce throttle to minimum before vehicle will disarm # define LAND_REQUIRE_MIN_THROTTLE_TO_DISARM ENABLED #endif diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 45d29e4f92e..2b599549e8b 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -191,8 +191,9 @@ static float get_throttle_land() // called at 50hz static void update_land_detector() { - // detect whether we have landed by watching for low climb rate and minimum throttle - if (abs(climb_rate) < 40 && motors.limit.throttle_lower) { + // detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate + if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && motors.limit.throttle_lower && + (motors.get_throttle_out() < get_non_takeoff_throttle()) && (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) { if (!ap.land_complete) { // increase counter until we hit the trigger then set land complete flag if( land_detector < LAND_DETECTOR_TRIGGER) { From cdc4038f27c363e676f52283dd8fa12bdd6240c1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Sep 2014 22:00:09 +0900 Subject: [PATCH 060/254] Copter: pre-arm consistency check of gyros --- ArduCopter/config.h | 5 +++++ ArduCopter/motors.pde | 16 ++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 33c781735dc..3cbbbd9cd5d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -309,6 +309,11 @@ #define PREARM_MAX_ACCEL_VECTOR_DIFF 1.0f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s #endif +// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros +#ifndef PREARM_MAX_GYRO_VECTOR_DIFF + #define PREARM_MAX_GYRO_VECTOR_DIFF 0.35f // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.35 rad/sec (=20deg/sec) +#endif + ////////////////////////////////////////////////////////////////////////////// // EKF Checker #ifndef EKFCHECK_THRESHOLD_DEFAULT diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index f6c63442f7e..a9ed222289f 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -374,6 +374,22 @@ static void pre_arm_checks(bool display_failure) } return; } + +#if INS_MAX_INSTANCES > 1 + // check all gyros are consistent + if (ins.get_gyro_count() > 1) { + for(uint8_t i=0; i PREARM_MAX_GYRO_VECTOR_DIFF) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros inconsistent")); + } + return; + } + } + } +#endif } #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 From fe07df556237fe6ba8f7323d26f6773b0858b4ac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Sep 2014 15:41:10 +0900 Subject: [PATCH 061/254] Cotper: AC3.2-rc7 release notes --- ArduCopter/ReleaseNotes.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 37d21dbdb92..cd1b4875400 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,13 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc7 04-Sep-2014 +Changes from 3.2-rc6 +1) Safety Items: + a) Landing check made more strict (climb rate requirement reduced to 30cm/s, overall throttle < 25%, rotation < 20deg/sec) + b) pre-arm check that accels are consistent (Pixhawk only, must be within 1m/sec/sec of each other) + c) pre-arm check that gyros are consistent (Pixhawk only, must be within 20deg/sec of each other) + d) report health of all accels and gyros (not just primary) to ground station +------------------------------------------------------------------ ArduCopter 3.2-rc6 31-Aug-2014 Changes from 3.2-rc5 1) Spline twitch when passing through a waypoint largely resolved From faf124771abaf65d2a45d7f7a7055b460403c947 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 4 Sep 2014 15:44:10 +0900 Subject: [PATCH 062/254] Copter: version to AC3.2-rc7 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 76b60cfddc6..09c6ab05de7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc6" +#define THISFIRMWARE "ArduCopter V3.2-rc7" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 62a4e66481e0e226292c5631353de7991490e8d7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Sep 2014 14:59:27 +0900 Subject: [PATCH 063/254] Copter: increase EKF_CHECK_THRESH default to 0.8 Also remove unused #define related to inertial nav check (now removed) --- ArduCopter/config.h | 2 +- ArduCopter/ekf_check.pde | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3cbbbd9cd5d..77c2e41b3bb 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -317,7 +317,7 @@ ////////////////////////////////////////////////////////////////////////////// // EKF Checker #ifndef EKFCHECK_THRESHOLD_DEFAULT - # define EKFCHECK_THRESHOLD_DEFAULT 0.6f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad + # define EKFCHECK_THRESHOLD_DEFAULT 0.8f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index bb4ae2ac630..11c59ac4868 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -11,10 +11,6 @@ # define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure #endif -#ifndef EKF_CHECK_COMPASS_INAV_CONVERSION - # define EKF_CHECK_COMPASS_INAV_CONVERSION 0.0075f // converts the inertial nav's acceleration corrections to a form that is comparable to the ekf variance -#endif - #ifndef EKF_CHECK_WARNING_TIME # define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds #endif From 4b47a618a4edaea617c837b46a0f868bdab20699 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 7 Sep 2014 22:14:40 +0900 Subject: [PATCH 064/254] Copter: reduce alt hold defaults Throttle Rate P to 5.0 (was 6.0) Throttle Accel P to 0.5 (was 0.75) Throttle Accel I to 1.0 (was 1.5) --- ArduCopter/config.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 77c2e41b3bb..fdd22d92ecd 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -685,7 +685,21 @@ // RATE control #ifndef THROTTLE_RATE_P - # define THROTTLE_RATE_P 6.0f + # define THROTTLE_RATE_P 5.0f +#endif + +// Throttle Accel control +#ifndef THROTTLE_ACCEL_P + # define THROTTLE_ACCEL_P 0.50f +#endif +#ifndef THROTTLE_ACCEL_I + # define THROTTLE_ACCEL_I 1.00f +#endif +#ifndef THROTTLE_ACCEL_D + # define THROTTLE_ACCEL_D 0.0f +#endif +#ifndef THROTTLE_ACCEL_IMAX + # define THROTTLE_ACCEL_IMAX 500 #endif // default maximum vertical velocity and acceleration the pilot may request @@ -705,20 +719,6 @@ # define ALT_HOLD_ACCEL_MAX 250 // if you change this you must also update the duplicate declaration in AC_WPNav.h #endif -// Throttle Accel control -#ifndef THROTTLE_ACCEL_P - # define THROTTLE_ACCEL_P 0.75f -#endif -#ifndef THROTTLE_ACCEL_I - # define THROTTLE_ACCEL_I 1.50f -#endif -#ifndef THROTTLE_ACCEL_D - # define THROTTLE_ACCEL_D 0.0f -#endif -#ifndef THROTTLE_ACCEL_IMAX - # define THROTTLE_ACCEL_IMAX 500 -#endif - ////////////////////////////////////////////////////////////////////////////// // Dataflash logging control // From 0aab3a024e857635080056971c44e5060bafda8f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Sep 2014 14:41:57 +0900 Subject: [PATCH 065/254] TradHeli: update AttControl params to match multicopters --- .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 9ae1b3ef8fb..72b582920d5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -10,7 +10,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @DisplayName: Angle Rate Roll-Pitch max // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes // @Units: Centi-Degrees/Sec - // @Range: 90000 250000 + // @Range: 9000 36000 // @Increment: 500 // @User: Advanced AP_GROUPINFO("RATE_RP_MAX", 0, AC_AttitudeControl_Heli, _angle_rate_rp_max, AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT), @@ -19,7 +19,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @DisplayName: Angle Rate Yaw max // @Description: maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes // @Units: Centi-Degrees/Sec - // @Range: 90000 250000 + // @Range: 4500 18000 // @Increment: 500 // @User: Advanced AP_GROUPINFO("RATE_Y_MAX", 1, AC_AttitudeControl_Heli, _angle_rate_y_max, AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT), @@ -37,8 +37,9 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @DisplayName: Acceleration Max for Roll/Pitch // @Description: Maximum acceleration in roll/pitch axis // @Units: Centi-Degrees/Sec/Sec - // @Range: 20000 100000 - // @Increment: 100 + // @Range: 0 180000 + // @Increment: 1000 + // @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast // @User: Advanced AP_GROUPINFO("ACCEL_RP_MAX", 3, AC_AttitudeControl_Heli, _accel_rp_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT), @@ -46,8 +47,9 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @DisplayName: Acceleration Max for Yaw // @Description: Maximum acceleration in yaw axis // @Units: Centi-Degrees/Sec/Sec - // @Range: 20000 100000 - // @Increment: 100 + // @Range: 0 72000 + // @Values: 0:Disabled, 18000:Slow, 36000:Medium, 54000:Fast + // @Increment: 1000 // @User: Advanced AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl_Heli, _accel_y_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT), From c8e652432df7a44e398654791a2f12ada9597664 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Sep 2014 17:32:35 +1000 Subject: [PATCH 066/254] DataFlash: allow use of a smaller writebuf for PX4v1 this fixes logging on PX4v1 --- libraries/DataFlash/DataFlash_File.cpp | 15 +++++++++++++-- libraries/DataFlash/DataFlash_File.h | 2 +- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 13a63ca3325..48c659e1b7d 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -105,9 +105,20 @@ void DataFlash_File::Init(const struct LogStructure *structure, uint8_t num_type if (_writebuf != NULL) { free(_writebuf); } - _writebuf = (uint8_t *)malloc(_writebuf_size); + + /* + if we can't allocate the full writebuf then try reducing it + until we can allocate it + */ + while (_writebuf == NULL && _writebuf_size >= _writebuf_chunk) { + _writebuf = (uint8_t *)malloc(_writebuf_size); + if (_writebuf == NULL) { + _writebuf_size /= 2; + } + } if (_writebuf == NULL) { - return; + hal.console->printf("Out of memory for logging\n"); + return; } _writebuf_head = _writebuf_tail = 0; _initialised = true; diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 0eda11f5988..1bd96571325 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -67,7 +67,7 @@ class DataFlash_File : public DataFlash_Class // write buffer uint8_t *_writebuf; - const uint16_t _writebuf_size; + uint16_t _writebuf_size; const uint16_t _writebuf_chunk; volatile uint16_t _writebuf_head; volatile uint16_t _writebuf_tail; From e706c2454243f53532136ee52ad6a0462cc06bde Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Sep 2014 22:17:46 +0900 Subject: [PATCH 067/254] Copter: send extended status to GCS only after initialisation --- ArduCopter/ArduCopter.pde | 1 + ArduCopter/GCS_Mavlink.pde | 12 ++++++++---- ArduCopter/system.pde | 3 +++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 09c6ab05de7..fa2f8d1b9de 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -388,6 +388,7 @@ static union { uint8_t rc_receiver_present : 1; // 14 // true if we have an rc receiver present (i.e. if we've ever received an update uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test + uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes }; uint32_t value; } ap; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8de62a6c134..ec3b5c5d5c0 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -481,10 +481,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) break; case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan); - CHECK_PAYLOAD_SIZE(POWER_STATUS); - gcs[chan-MAVLINK_COMM_0].send_power_status(); + // send extended status only once vehicle has been initialised + // to avoid unnecessary errors being reported to user + if (ap.initialised) { + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan); + CHECK_PAYLOAD_SIZE(POWER_STATUS); + gcs[chan-MAVLINK_COMM_0].send_power_status(); + } break; case MSG_EXTENDED_STATUS2: diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 89c0f5df540..e0fa98560df 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -295,6 +295,9 @@ static void init_ardupilot() } cliSerial->print_P(PSTR("\nReady to FLY ")); + + // flag that initialisation has completed + ap.initialised = true; } From cac10a3041b2c0bbf6e221c94444b8f3f97f4fdd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Sep 2014 22:19:20 +0900 Subject: [PATCH 068/254] Copter: never send unhealthy terrain status Copter does not yet rely on the terrain data (it's for informational purposes only) so we will temporarily disable the failure flags to the GCS to avoid support calls --- ArduCopter/GCS_Mavlink.pde | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index ec3b5c5d5c0..9c6f3d30468 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -224,9 +224,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) case AP_Terrain::TerrainStatusDisabled: break; case AP_Terrain::TerrainStatusUnhealthy: - control_sensors_present |= MAV_SYS_STATUS_TERRAIN; - control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; - break; + // To-Do: restore unhealthy terrain status reporting once terrain is used in copter + //control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + //break; case AP_Terrain::TerrainStatusOK: control_sensors_present |= MAV_SYS_STATUS_TERRAIN; control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; From 01536e0c8081003849c9000110738d515a6dfbbc Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 6 Sep 2014 06:33:47 +1000 Subject: [PATCH 069/254] AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 101 +++++++++++++++--------------- libraries/AP_NavEKF/AP_NavEKF.h | 2 +- 2 files changed, 52 insertions(+), 51 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 931d20a17c9..c07b573e446 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -369,7 +369,7 @@ bool NavEKF::healthy(void) const if (state.velocity.is_nan()) { return false; } - if (filterDiverged || (hal.scheduler->millis() - lastDivergeTime_ms < 10000)) { + if (filterDiverged || (imuSampleTime_ms - lastDivergeTime_ms < 10000)) { return false; } // If measurements have failed innovation consistency checks for long enough to time-out @@ -677,12 +677,12 @@ void NavEKF::SelectVelPosFusion() // If a long time since last GPS update, then reset position and velocity and reset stored state history uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS; - if (hal.scheduler->millis() - secondLastFixTime_ms > gpsRetryTimeout) { + if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) { ResetPosition(); ResetVelocity(); StoreStatesReset(); } - } else if (hal.scheduler->millis() - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { + } else if (imuSampleTime_ms - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same // measurement until the next one arrives to provide a smoother output fuseVelData = false; @@ -696,7 +696,7 @@ void NavEKF::SelectVelPosFusion() newDataHgt = false; // enable fusion fuseHgtData = true; - } else if (hal.scheduler->millis() - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { + } else if (imuSampleTime_ms - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { // timeout fusion of height data if stale. Needed because we repeatedly fuse the same // measurement until the next one arrives to provide a smoother output fuseHgtData = false; @@ -743,9 +743,9 @@ void NavEKF::SelectMagFusion() // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout // If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset if (magHealth) { - lastHealthyMagTime_ms = hal.scheduler->millis(); + lastHealthyMagTime_ms = imuSampleTime_ms; } else { - if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) { + if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) { magTimeout = true; if (assume_zero_sideslip()) { magFailed = true; @@ -759,7 +759,7 @@ void NavEKF::SelectMagFusion() bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) { - MAGmsecPrev = IMUmsec; + MAGmsecPrev = imuSampleTime_ms; fuseMagData = true; } else @@ -782,7 +782,7 @@ void NavEKF::SelectTasFusion() tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas)); // if we have waited too long, set a timeout flag which will force fusion to occur - bool timeout = ((IMUmsec - TASmsecPrev) >= TASmsecMax); + bool timeout = ((imuSampleTime_ms - TASmsecPrev) >= TASmsecMax); // we don't fuse airspeed measurements if magnetometer fusion has been performed in the same frame, unless timed out or the fuseMeNow option is selected // this helps to spreasthe load associated with fusion of different measurements across multiple frames @@ -790,7 +790,7 @@ void NavEKF::SelectTasFusion() if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow)) { FuseAirspeed(); - TASmsecPrev = IMUmsec; + TASmsecPrev = imuSampleTime_ms; tasDataWaiting = false; } } @@ -805,9 +805,9 @@ void NavEKF::SelectBetaFusion() // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position // AND NOT on the ground AND enough time has lapsed since our last fusion // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set) - if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) { + if (assume_zero_sideslip() && !(use_compass() && useAirspeed() && posHealth) && !inhibitWindStates && ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) { FuseSideslip(); - BETAmsecPrev = IMUmsec; + BETAmsecPrev = imuSampleTime_ms; } } @@ -1676,15 +1676,15 @@ void NavEKF::FuseVelPosNED() // calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring float accelScale = (1.0f + 0.1f * accNavMagHoriz); - float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime))); + float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data - posTimeout = ((hal.scheduler->millis() - posFailTime) > gpsRetryTime); + posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime); // use position data if healthy, timed out, or in static mode if (posHealth || posTimeout || staticMode) { posHealth = true; - posFailTime = hal.scheduler->millis(); + posFailTime = imuSampleTime_ms; // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { gpsPosGlitchOffsetNE.x += posInnov[0]; @@ -1745,11 +1745,11 @@ void NavEKF::FuseVelPosNED() // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // declare a timeout if we have not fused velocity data for too long - velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime; + velTimeout = (imuSampleTime_ms - velFailTime) > gpsRetryTime; // if data is healthy or in static mode we fuse it if (velHealth || staticMode) { velHealth = true; - velFailTime = hal.scheduler->millis(); + velFailTime = imuSampleTime_ms; } else if (velTimeout && !posHealth) { // if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step ResetVelocity(); @@ -1774,11 +1774,11 @@ void NavEKF::FuseVelPosNED() hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); - hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime; + hgtTimeout = (imuSampleTime_ms - hgtFailTime) > hgtRetryTime; // Fuse height data if healthy or timed out or in static mode if (hgtHealth || hgtTimeout || staticMode) { hgtHealth = true; - hgtFailTime = hal.scheduler->millis(); + hgtFailTime = imuSampleTime_ms; // if timed out, reset the height, but do not fuse data on this time step if (hgtTimeout) { ResetHeight(); @@ -2678,8 +2678,8 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last) void NavEKF::StoreStates() { // Don't need to store states more often than every 10 msec - if (hal.scheduler->millis() - lastStateStoreTime_ms >= 10) { - lastStateStoreTime_ms = hal.scheduler->millis(); + if (imuSampleTime_ms - lastStateStoreTime_ms >= 10) { + lastStateStoreTime_ms = imuSampleTime_ms; if (storeIndex > 49) { storeIndex = 0; } @@ -2698,7 +2698,7 @@ void NavEKF::StoreStatesReset() // store current state vector in first column storeIndex = 0; storedStates[storeIndex] = state; - statetimeStamp[storeIndex] = hal.scheduler->millis(); + statetimeStamp[storeIndex] = imuSampleTime_ms; storeIndex = storeIndex + 1; } @@ -2922,7 +2922,7 @@ void NavEKF::ForceSymmetry() // set the filter status as diverged and re-initialise the filter filterDiverged = true; faultStatus.diverged = true; - lastDivergeTime_ms = hal.scheduler->millis(); + lastDivergeTime_ms = imuSampleTime_ms; InitialiseFilterDynamic(); return; } @@ -2993,8 +2993,8 @@ void NavEKF::readIMUData() Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2) Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2) - // get the time the IMU data was read - IMUmsec = hal.scheduler->millis(); + // the imu sample time is sued as a common time reference throughout the filter + imuSampleTime_ms = hal.scheduler->millis(); // limit IMU delta time to prevent numerical problems elsewhere dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f); @@ -3048,8 +3048,8 @@ void NavEKF::readGpsData() // get state vectors that were stored at the time that is closest to when the the GPS measurement // time after accounting for measurement delays - RecallStates(statesAtVelTime, (IMUmsec - constrain_int16(_msecVelDelay, 0, 500))); - RecallStates(statesAtPosTime, (IMUmsec - constrain_int16(_msecPosDelay, 0, 500))); + RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500))); + RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500))); // read the NED velocity from the GPS velNED = _ahrs->get_gps().velocity(); @@ -3079,14 +3079,14 @@ void NavEKF::readHgtData() lastHgtMeasTime = _baro.get_last_update(); // time stamp used to check for timeout - lastHgtTime_ms = hal.scheduler->millis(); + lastHgtTime_ms = imuSampleTime_ms; // get measurement and set flag to let other functions know new data has arrived hgtMea = _baro.get_altitude(); newDataHgt = true; // get states that wer stored at the time closest to the measurement time, taking measurement delay into account - RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay)); + RecallStates(statesAtHgtTime, (imuSampleTime_ms - _msecHgtDelay)); } else { newDataHgt = false; } @@ -3106,7 +3106,7 @@ void NavEKF::readMagData() magData = _ahrs->get_compass()->get_field() * 0.001f + magBias; // get states stored at time closest to measurement time after allowance for measurement delay - RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay)); + RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay)); // let other processes know that new compass data has arrived newDataMag = true; @@ -3128,7 +3128,7 @@ void NavEKF::readAirSpdData() VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); lastAirspeedUpdate = aspeed->last_update_ms(); newDataTas = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _msecTasDelay)); + RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - _msecTasDelay)); } else { newDataTas = false; } @@ -3308,29 +3308,30 @@ void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f // zero stored variables - this needs to be called before a full filter initialisation void NavEKF::ZeroVariables() { + // initialise time stamps + imuSampleTime_ms = hal.scheduler->millis(); + lastHealthyMagTime_ms = imuSampleTime_ms; + lastDivergeTime_ms = imuSampleTime_ms; + TASmsecPrev = imuSampleTime_ms; + BETAmsecPrev = imuSampleTime_ms; + lastMagUpdate = imuSampleTime_ms; + lastHgtMeasTime = imuSampleTime_ms; + lastHgtTime_ms = imuSampleTime_ms; + velFailTime = imuSampleTime_ms; + posFailTime = imuSampleTime_ms; + hgtFailTime = imuSampleTime_ms; + lastStateStoreTime_ms = imuSampleTime_ms; + lastFixTime_ms = imuSampleTime_ms; + secondLastFixTime_ms = imuSampleTime_ms; + lastDecayTime_ms = imuSampleTime_ms; + velTimeout = false; posTimeout = false; hgtTimeout = false; filterDiverged = false; magTimeout = false; magFailed = false; - lastHealthyMagTime_ms = hal.scheduler->millis(); - lastStateStoreTime_ms = 0; - lastFixTime_ms = 0; - secondLastFixTime_ms = 0; - lastMagUpdate = 0; - lastAirspeedUpdate = 0; - velFailTime = 0; - posFailTime = 0; - hgtFailTime = 0; storeIndex = 0; - TASmsecPrev = 0; - BETAmsecPrev = 0; - MAGmsecPrev = 0; - HGTmsecPrev = 0; - lastMagUpdate = 0; - lastAirspeedUpdate = 0; - lastHgtMeasTime = 0; dtIMU = 0; dt = 0; hgtMea = 0; @@ -3382,8 +3383,8 @@ bool NavEKF::use_compass(void) const // apply glitch offset to GPS measurements void NavEKF::decayGpsOffset() { - float lapsedTime = 0.001f*float(hal.scheduler->millis() - lastDecayTime_ms); - lastDecayTime_ms = hal.scheduler->millis(); + float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms); + lastDecayTime_ms = imuSampleTime_ms; float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y); // decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero) if (offsetRadius > (lapsedTime + 0.1f)) { @@ -3419,11 +3420,11 @@ void NavEKF::checkDivergence() } bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); lastGyroBias = state.gyro_bias; - if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) { + if (imuSampleTime_ms - lastDivergeTime_ms > 10000) { if (divergenceDetected) { filterDiverged = true; faultStatus.diverged = true; - lastDivergeTime_ms = hal.scheduler->millis(); + lastDivergeTime_ms = imuSampleTime_ms; } else { filterDiverged = false; } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 7f3252da311..e628f490ec0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -431,7 +431,7 @@ class NavEKF Vector3f velDotNED; // rate of change of velocity in NED frame Vector3f velDotNEDfilt; // low pass filtered velDotNED uint32_t lastAirspeedUpdate; // last time airspeed was updated - uint32_t IMUmsec; // time that the last IMU value was taken + uint32_t imuSampleTime_ms; // time that the last IMU value was taken ftype gpsCourse; // GPS ground course angle(rad) ftype gpsGndSpd; // GPS ground speed (m/s) bool newDataGps; // true when new GPS data has arrived From 058fb8f3ee6fea907785290bc19a061175ec0959 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 24 Aug 2014 12:02:29 +1000 Subject: [PATCH 070/254] AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 156 ++++++++++++++++++++---------- libraries/AP_NavEKF/AP_NavEKF.h | 21 +++- 2 files changed, 120 insertions(+), 57 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index c07b573e446..398b7ac7e36 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -38,9 +38,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0001f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 5 +#define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 @@ -59,9 +59,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 5 +#define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 @@ -80,9 +80,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 10 +#define HGT_GATE_DEFAULT 20 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 0 #define GLITCH_ACCEL_DEFAULT 150 @@ -346,6 +346,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground _msecGpsAvg = 200; // average number of msec between GPS measurements _msecHgtAvg = 100; // average number of msec between height measurements + _msecMagAvg = 100; // average number of msec between magnetometer measurements _msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. // Misc initial conditions @@ -353,7 +354,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : mag_state.q0 = 1; mag_state.DCM.identity(); IMU1_weighting = 0.5f; - lastDivergeTime_ms = 0; memset(&faultStatus, 0, sizeof(faultStatus)); } @@ -411,6 +411,8 @@ void NavEKF::ResetPosition(void) states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x; states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y; } + // reset the glitch ofset correction states + gpsPosGlitchOffsetNE.zero(); } // resets velocity states to last GPS measurement or to zero if in static mode @@ -464,7 +466,15 @@ void NavEKF::InitialiseFilterDynamic(void) ZeroVariables(); // get initial time deltat between IMU measurements (sec) - dtIMU = _ahrs->get_ins().get_delta_time(); + dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + + // set number of updates over which gps and baro measurements are applied to the velocity and position states + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); // calculate initial orientation and earth magnetic field states Quaternion initQuat; @@ -501,6 +511,17 @@ void NavEKF::InitialiseFilterBootstrap(void) // set re-used variables to zero ZeroVariables(); + // get initial time deltat between IMU measurements (sec) + dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + + // set number of updates over which gps and baro measurements are applied to the velocity and position states + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); + // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f initAccVec; @@ -648,17 +669,6 @@ void NavEKF::UpdateFilter() // select fusion of velocity, position and height measurements void NavEKF::SelectVelPosFusion() { - // calculate ratio of VelPos fusion to state prediction steps - uint8_t velPosFuseStepRatio = floor(dtVelPos/dtIMU + 0.5f); - - // calculate the scale factor to be applied to GPS measurement variance to account for - // the fact we repeat fusion of the same measurement to provide a smoother output - gpsVarScaler = _msecGpsAvg/(1000.0f*dtVelPos); - - // calculate the scale factor to be applied to height measurement variance to account for - // the fact we repeat fusion of the same measurement to provide a smoother output - hgtVarScaler = _msecHgtAvg/(1000.0f*dtVelPos); - // check for new data, specify which measurements should be used and check data for freshness if (!staticMode) { @@ -669,36 +679,38 @@ void NavEKF::SelectVelPosFusion() if (newDataGps) { // reset data arrived flag newDataGps = false; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta)); + gpsUpdateCount = 0; // enable fusion fuseVelData = true; fusePosData = true; - // reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives - skipCounter = velPosFuseStepRatio; // If a long time since last GPS update, then reset position and velocity and reset stored state history - uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS; if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) { ResetPosition(); ResetVelocity(); StoreStatesReset(); } - } else if (imuSampleTime_ms - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { - // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same - // measurement until the next one arrives to provide a smoother output + } else { fuseVelData = false; fusePosData = false; } + // check for and read new height data + readHgtData(); + // command fusion of height data if (newDataHgt) { // reset data arrived flag newDataHgt = false; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); + hgtUpdateCount = 0; // enable fusion fuseHgtData = true; - } else if (imuSampleTime_ms - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { - // timeout fusion of height data if stale. Needed because we repeatedly fuse the same - // measurement until the next one arrives to provide a smoother output + } else { fuseHgtData = false; } @@ -715,22 +727,24 @@ void NavEKF::SelectVelPosFusion() fuseHgtData = true; } - // check for and read new height data - readHgtData(); - - // perform fusion as commanded, and in accordance with specified time intervals + // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { - // skip fusion as required to maintain ~dtVelPos time interval between corrections - if (skipCounter >= velPosFuseStepRatio) { FuseVelPosNED(); - // reset counter used to skip update frames - skipCounter = 1; - } else { - // increment counter used to skip update frames - skipCounter += 1; - } } + // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 5 and 10Hz pulsing in the output + if (gpsUpdateCount < gpsUpdateCountMax) { + gpsUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += gpsIncrStateDelta[i]; + } + } + if (hgtUpdateCount < hgtUpdateCountMax) { + hgtUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += hgtIncrStateDelta[i]; + } + } } // select fusion of magnetometer data @@ -759,8 +773,10 @@ void NavEKF::SelectMagFusion() bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) { - MAGmsecPrev = imuSampleTime_ms; fuseMagData = true; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); + magUpdateCount = 0; } else { @@ -769,6 +785,15 @@ void NavEKF::SelectMagFusion() // call the function that performs fusion of magnetometer data FuseMagnetometer(); + + // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output + if (magUpdateCount < magUpdateCountMax) { + magUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += magIncrStateDelta[i]; + } + } + } } @@ -1641,20 +1666,20 @@ void NavEKF::FuseVelPosNED() posErr = _gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. - R_OBS[0] = gpsVarScaler*(sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr)); + R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = gpsVarScaler*(sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr)); - R_OBS[3] = gpsVarScaler*(sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr)); + R_OBS[2] = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr); + R_OBS[3] = sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; - R_OBS[5] = hgtVarScaler*sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); + R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. bool badIMUdata = false; - if (_fusionModeGPS == 0 && fuseVelData && fuseHgtData) { + if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * _msecHgtAvg)) { // calculate innovations for height and vertical GPS vel measurements - float hgtErr = statesAtVelTime.position.z - observation[5]; + float hgtErr = statesAtHgtTime.position.z - observation[5]; float velDErr = statesAtVelTime.velocity.z - observation[2]; // check if they are the same sign and both more than 3-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS[2]))) { @@ -1890,9 +1915,21 @@ void NavEKF::FuseVelPosNED() } } - // calculate state corrections and re-normalise the quaternions for blended IMU data predicted states + // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data + // attitude, velocity and position corrections are spread across multiple prediction cycles between now + // and the anticipated time for the next measurement. + // Don't spread quaternion corrections if total angle change across predicted interval is going to exceed 0.1 rad + bool highRates = ((gpsUpdateCountMax * correctedDelAng.length()) > 0.1f); for (uint8_t i = 0; i<=21; i++) { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + if ((i <= 3 && highRates) || i >= 10 || staticMode) { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } else { + if (obsIndex == 5) { + hgtIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * hgtUpdateCountMaxInv; + } else { + gpsIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * gpsUpdateCountMaxInv; + } + } } state.quat.normalize(); @@ -1913,7 +1950,7 @@ void NavEKF::FuseVelPosNED() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); @@ -2236,13 +2273,24 @@ void NavEKF::FuseMagnetometer() // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle // In this case we might as well try using the magnetometer, but with a reduced weighting if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) { - // correct the state vector + // Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement. + // Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad + bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f); + // Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles + // There is no point averaging if the number of cycles left is less than 2 + float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount); + // correct the state vector or store corrections to be applied incrementally for (uint8_t j= 0; j<=21; j++) { // If we are forced to use a bad compass, we reduce the weighting by a factor of 4 if (!magHealth) { Kfusion[j] *= 0.25f; } - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + if ((j <= 3 && highRates) || j >= 10 || staticMode || minorFramesToGo < 1.5f ) { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } else { + // scale the correction based on the number of averaging frames left to go + magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo); + } } // normalise the quaternion states state.quat.normalize(); @@ -3353,6 +3401,10 @@ void NavEKF::ZeroVariables() memset(&processNoise[0], 0, sizeof(processNoise)); memset(&storedStates[0], 0, sizeof(storedStates)); memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta)); + memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); + memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); + gpsPosGlitchOffsetNE.zero(); } // return true if we should use the airspeed sensor diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index e628f490ec0..5e7e68bc2ed 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -347,11 +347,11 @@ class NavEKF float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate uint16_t _msecGpsAvg; // average number of msec between GPS measurements uint16_t _msecHgtAvg; // average number of msec between height measurements + uint16_t _msecMagAvg; // average number of msec between magnetometer measurements uint16_t _msecBetaAvg; // maximum number of msec between synthetic sideslip measurements float dtVelPos; // average of msec between position and velocity corrections // Variables - uint8_t skipCounter; // counter used to skip position and height corrections to achieve _skipRatio bool statesInitialised; // boolean true when filter states have been initialised bool velHealth; // boolean true if velocity measurements have passed innovation consistency check bool posHealth; // boolean true if position measurements have passed innovation consistency check @@ -422,8 +422,6 @@ class NavEKF uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps - uint32_t MAGmsecPrev; // time stamp of last compass fusion step - uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step const bool fuseMeNow; // boolean to force fusion whenever data arrives bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing bool prevStaticMode; // value of static mode from last update @@ -436,13 +434,11 @@ class NavEKF ftype gpsGndSpd; // GPS ground speed (m/s) bool newDataGps; // true when new GPS data has arrived bool newDataMag; // true when new magnetometer data has arrived - float gpsVarScaler; // scaler applied to gps measurement variance to allow for oversampling bool newDataTas; // true when new airspeed data has arrived bool tasDataWaiting; // true when new airspeed data is waiting to be fused bool newDataHgt; // true when new height data has arrived uint32_t lastHgtMeasTime; // time of last height measurement used to determine if new data has arrived uint32_t lastHgtTime_ms; // time of last height update (msec) used to calculate timeout - float hgtVarScaler; // scaler applied to height measurement variance to allow for oversampling uint32_t velFailTime; // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec) uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec) uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec) @@ -471,6 +467,21 @@ class NavEKF float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant + + // Used by smoothing of state corrections + float gpsIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement + float hgtIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next height measurement + float magIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement + uint8_t gpsUpdateCount; // count of the number of minor state corrections using GPS data + uint8_t gpsUpdateCountMax; // limit on the number of minor state corrections using GPS data + float gpsUpdateCountMaxInv; // floating point inverse of gpsFilterCountMax + uint8_t hgtUpdateCount; // count of the number of minor state corrections using Baro data + uint8_t hgtUpdateCountMax; // limit on the number of minor state corrections using Baro data + float hgtUpdateCountMaxInv; // floating point inverse of hgtFilterCountMax + uint8_t magUpdateCount; // count of the number of minor state corrections using Magnetometer data + uint8_t magUpdateCountMax; // limit on the number of minor state corrections using Magnetometer data + float magUpdateCountMaxInv; // floating point inverse of magFilterCountMax + struct { bool diverged:1; bool large_covarience:1; From 1ed11c7c37df65b47a93e1cd5aaca0014b538a8d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Sep 2014 16:34:37 +0900 Subject: [PATCH 071/254] Copter: AC3.2-rc8 release notes --- ArduCopter/ReleaseNotes.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index cd1b4875400..5722a778b6b 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,17 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc8 11-Sep-2014 +Changes from 3.2-rc7 +1) EKF reduced ripple to resolve copter motor pulsing +2) Default Param changes: + a) AltHold Rate P reduced from 6 to 5 + b) AltHold Accel P reduced from 0.75 to 0.5, I from 1.5 to 1.0 + c) EKF check threshold increased from 0.6 to 0.8 to reduce false positives +3) sensor health flags sent to GCS only after initialisation to remove false alerts +4) suppress bad terrain data alerts +5) Bug Fix: + a)PX4 dataflash RAM useage reduced to 8k so it works again +------------------------------------------------------------------ ArduCopter 3.2-rc7 04-Sep-2014 Changes from 3.2-rc6 1) Safety Items: From 12f3e96cc191b25ece91c36c7a2977df1d66f4ae Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Sep 2014 16:36:43 +0900 Subject: [PATCH 072/254] Copter: version to AC3.2-rc8 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fa2f8d1b9de..5730356618c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc7" +#define THISFIRMWARE "ArduCopter V3.2-rc8" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From d24159204ec82b1d266da89f70c39c50b00c89c8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Sep 2014 18:52:31 +1000 Subject: [PATCH 073/254] HAL_PX4: fixed dirty_mask calculation in FRAM storage this could lead to a number of bytes on 512 byte boundaries not being written when changed in ram, so they would revert on next boot --- libraries/AP_HAL_PX4/Storage.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp index 1fc54b591a2..b895b39fd76 100644 --- a/libraries/AP_HAL_PX4/Storage.cpp +++ b/libraries/AP_HAL_PX4/Storage.cpp @@ -176,12 +176,12 @@ void PX4Storage::_storage_open(void) */ void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length) { - uint16_t end = loc + length; - while (loc < end) { - uint8_t line = (loc >> PX4_STORAGE_LINE_SHIFT); - _dirty_mask |= 1 << line; - loc += PX4_STORAGE_LINE_SIZE; - } + uint16_t end = loc + length; + for (uint8_t line=loc>>PX4_STORAGE_LINE_SHIFT; + line <= end>>PX4_STORAGE_LINE_SHIFT; + line++) { + _dirty_mask |= 1U << line; + } } void PX4Storage::read_block(void *dst, uint16_t loc, size_t n) From 939df78a2fdfe6ddc29ae147cfa052e381682c19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 11 Sep 2014 18:57:41 +1000 Subject: [PATCH 074/254] HAL_VRBRAIN: fixed storage bug in VRBRAIN too --- libraries/AP_HAL_VRBRAIN/Storage.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_VRBRAIN/Storage.cpp b/libraries/AP_HAL_VRBRAIN/Storage.cpp index f83af000f82..9d2d2e76e71 100644 --- a/libraries/AP_HAL_VRBRAIN/Storage.cpp +++ b/libraries/AP_HAL_VRBRAIN/Storage.cpp @@ -177,11 +177,11 @@ void VRBRAINStorage::_storage_open(void) void VRBRAINStorage::_mark_dirty(uint16_t loc, uint16_t length) { uint16_t end = loc + length; - while (loc < end) { - uint8_t line = (loc >> VRBRAIN_STORAGE_LINE_SHIFT); - _dirty_mask |= 1 << line; - loc += VRBRAIN_STORAGE_LINE_SIZE; - } + for (uint8_t line=loc>>VRBRAIN_STORAGE_LINE_SHIFT; + line <= end>>VRBRAIN_STORAGE_LINE_SHIFT; + line++) { + _dirty_mask |= 1U << line; + } } void VRBRAINStorage::read_block(void *dst, uint16_t loc, size_t n) From a7233c48be23f1884d18a2d319449ec8c6111190 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Sep 2014 20:28:35 +0900 Subject: [PATCH 075/254] Copter: AC3.2-rc9 release notes --- ArduCopter/ReleaseNotes.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 5722a778b6b..6a2199ca367 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,9 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc9 11-Sep-2014 +Changes from 3.2-rc8 +1) FRAM bug fix that could stop Mission or Parameter changes from being saved (Pixhawk, VRBrain only) +------------------------------------------------------------------ ArduCopter 3.2-rc8 11-Sep-2014 Changes from 3.2-rc7 1) EKF reduced ripple to resolve copter motor pulsing From 75a1e46d82ea00ba3b00977f8ab9c0c400beafc0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Sep 2014 20:28:43 +0900 Subject: [PATCH 076/254] Copter: version to AC3.2-rc9 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5730356618c..10a88b18849 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc8" +#define THISFIRMWARE "ArduCopter V3.2-rc9" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 64cc4986bdc90e27ff649e12885b9bcc2dc3fc93 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Sep 2014 14:02:06 +0900 Subject: [PATCH 077/254] Copter: THR_ACCEL_IMAX param range increased --- ArduCopter/Parameters.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 09a0e99d645..f894f0a0cd6 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -808,7 +808,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: THR_ACCEL_IMAX // @DisplayName: Throttle acceleration controller I gain maximum // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate - // @Range: 0 500 + // @Range: 0 1000 // @Units: Percent*10 // @User: Standard From 5fd39ce436d800949f2ff85688d27e26e7503a5f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Sep 2014 14:15:38 +0900 Subject: [PATCH 078/254] Copter: reduce throttle to min once landed in RTL This catches the case where the vehicle lands but the user doesn't immediately put the throttle to zero. Before this check it would continue to attempt to hold it's --- ArduCopter/control_rtl.pde | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 88689b85b5e..2caafe4e924 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -325,12 +325,27 @@ static void rtl_land_run() int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed || !inertial_nav.position_ok()) { + if(!ap.auto_armed || ap.land_complete) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // set target to current position wp_nav.init_loiter_target(); + +#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED + // disarm when the landing detector says we've landed and throttle is at minimum + if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + init_disarm_motors(); + } +#else + // disarm when the landing detector says we've landed + if (ap.land_complete) { + init_disarm_motors(); + } +#endif + + // check if we've completed this stage of RTL + rtl_state_complete = ap.land_complete; return; } @@ -365,18 +380,6 @@ static void rtl_land_run() // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; - -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { - init_disarm_motors(); - } -#else - // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); - } -#endif } // get_RTL_alt - return altitude which vehicle should return home at From 0a3ec84e866eab92ab88ecde7a094833da011831 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Sep 2014 15:04:29 +0900 Subject: [PATCH 079/254] Copter: THR_ACCEL_IMAX default to 800 --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index fdd22d92ecd..a1159c0adcc 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -699,7 +699,7 @@ # define THROTTLE_ACCEL_D 0.0f #endif #ifndef THROTTLE_ACCEL_IMAX - # define THROTTLE_ACCEL_IMAX 500 + # define THROTTLE_ACCEL_IMAX 800 #endif // default maximum vertical velocity and acceleration the pilot may request From b212c02057c2c0e3f11fa32cb218bfc6e2029c38 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Sep 2014 15:06:24 +0900 Subject: [PATCH 080/254] Copter: version to AC3.2-Iris Interim release 3DRobotics's use with IRIS+ frames --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 10a88b18849..2204bbbc01c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc9" +#define THISFIRMWARE "ArduCopter V3.2-Iris" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 3993927cb76832746898e74e9927cf84998e05d6 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 15 Sep 2014 17:16:41 -0700 Subject: [PATCH 081/254] Copter: Log NTUN while in LAND mode with GPS --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/control_land.pde | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 2204bbbc01c..ddec25f849e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1112,7 +1112,7 @@ static void ten_hz_logging_loop() if (g.log_bitmask & MASK_LOG_RCOUT) { DataFlash.Log_Write_RCOUT(); } - if ((g.log_bitmask & MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) { + if ((g.log_bitmask & MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { Log_Write_Nav_Tuning(); } } diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 2b599549e8b..237aad939d8 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -227,3 +227,8 @@ static void set_mode_land_with_pause() set_mode(LAND); land_pause = true; } + +// landing_with_GPS - returns true if vehicle is landing using GPS +static bool landing_with_GPS() { + return (control_mode == LAND && land_with_gps); +} From bf1ccba742161b6f00bba428cdb5e05368ab6798 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 15 Sep 2014 18:09:10 +1000 Subject: [PATCH 082/254] AP_NavEKF : Reduce sensitivity on filter divergence check Flying aerobatics with Trad Heli has shown that the divergence check can be false triggered when large magnetometer errors and GPS dropouts are present. This can also happen with multi rotors if large yaw rates are present. This was an unintended consequence of the ekfsmoothing patch which improved filter stability during high rate manoeuvres, but made the divergence test more sensitive. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 398b7ac7e36..a0742a0ac9a 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3468,7 +3468,7 @@ void NavEKF::checkDivergence() float tempLength = tempVec.length(); if (tempLength != 0.0f) { float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f); - scaledDeltaGyrBiasLgth = (5e-7f / temp) * tempVec.length() / dtIMU; + scaledDeltaGyrBiasLgth = (5e-8f / temp) * tempVec.length() / dtIMU; } bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); lastGyroBias = state.gyro_bias; From be2f3088020eb35c06cca623533e00499f257e37 Mon Sep 17 00:00:00 2001 From: Andre Kjellstrup Date: Wed, 17 Sep 2014 12:28:02 +0200 Subject: [PATCH 083/254] Copter: reset battery_fs after dis/rearming --- ArduCopter/motors.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index a9ed222289f..2f3681ac8b9 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -132,6 +132,9 @@ static void init_arm_motors() // disable inertial nav errors temporarily inertial_nav.ignore_next_error(); + // reset battery failsafe + set_failsafe_battery(false); + // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out From b8c74b7363f80b57f7ae0fbae3bc7965b0944462 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Sep 2014 17:24:40 +0900 Subject: [PATCH 084/254] Copter: define limit for baro vs inav alt disparity --- ArduCopter/config.h | 5 +++++ ArduCopter/motors.pde | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a1159c0adcc..628bf53e1c1 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -299,6 +299,11 @@ #define FS_GCS_ENABLED_ALWAYS_RTL 1 #define FS_GCS_ENABLED_CONTINUE_MISSION 2 +// pre-arm baro vs inertial nav max alt disparity +#ifndef PREARM_MAX_ALT_DISPARITY_CMS + # define PREARM_MAX_ALT_DISPARITY_CMS 100 // barometer and inertial nav altitude must be within this many centimeters +#endif + // pre-arm check max velocity #ifndef PREARM_MAX_VELOCITY_CMS # define PREARM_MAX_VELOCITY_CMS 50.0f // vehicle must be travelling under 50cm/s before arming diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2f3681ac8b9..21f2efd7347 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -536,7 +536,7 @@ static bool arm_checks(bool display_failure) // check Baro & inav alt are within 1m if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { - if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) { + if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CMS) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity")); } From a1ea43461ad0c076d53f69c373a4e934af7a4f5f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Sep 2014 17:32:03 +0900 Subject: [PATCH 085/254] Copter: typo fix for baro vs inav alt disparity definition --- ArduCopter/config.h | 4 ++-- ArduCopter/motors.pde | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 628bf53e1c1..13134f1e1a3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -300,8 +300,8 @@ #define FS_GCS_ENABLED_CONTINUE_MISSION 2 // pre-arm baro vs inertial nav max alt disparity -#ifndef PREARM_MAX_ALT_DISPARITY_CMS - # define PREARM_MAX_ALT_DISPARITY_CMS 100 // barometer and inertial nav altitude must be within this many centimeters +#ifndef PREARM_MAX_ALT_DISPARITY_CM + # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters #endif // pre-arm check max velocity diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 21f2efd7347..65af49887a7 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -536,7 +536,7 @@ static bool arm_checks(bool display_failure) // check Baro & inav alt are within 1m if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { - if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CMS) { + if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity")); } From ab4b545bb5f3d579c1ecc019a2a6d4839f53edf0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Sep 2014 15:39:08 +0900 Subject: [PATCH 086/254] AC_WPNav: add loiter_soften_for_landing method This resets the position target to the current location. --- libraries/AC_WPNav/AC_WPNav.cpp | 10 ++++++++++ libraries/AC_WPNav/AC_WPNav.h | 3 +++ 2 files changed, 13 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 16f46d23dbe..1fa531be4a4 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -176,6 +176,16 @@ void AC_WPNav::init_loiter_target() _pilot_accel_rgt_cms = 0; } +/// loiter_soften_for_landing - reduce response for landing +void AC_WPNav::loiter_soften_for_landing() +{ + const Vector3f& curr_pos = _inav.get_position(); + + // set target position to current position + _pos_control.set_xy_target(curr_pos.x, curr_pos.y); + _pos_control.freeze_ff_xy(); +} + /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter void AC_WPNav::set_loiter_velocity(float velocity_cms) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 0342889f420..985f1206fae 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -69,6 +69,9 @@ class AC_WPNav /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity void init_loiter_target(); + /// loiter_soften_for_landing - reduce response for landing + void loiter_soften_for_landing(); + /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter void set_loiter_velocity(float velocity_cms); From 7f9709300cdd18e33b1b316d5597b54a7799b854 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Aug 2014 15:25:37 +0900 Subject: [PATCH 087/254] Copter: soften loiter target when maybe landed Applies to auto's land, land, loiter, pos hold and rtl flight modes --- ArduCopter/config.h | 3 +++ ArduCopter/control_auto.pde | 5 +++++ ArduCopter/control_land.pde | 10 ++++++++++ ArduCopter/control_loiter.pde | 5 +++++ ArduCopter/control_poshold.pde | 5 +++++ ArduCopter/control_rtl.pde | 5 +++++ 6 files changed, 33 insertions(+) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 13134f1e1a3..60475602062 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -462,6 +462,9 @@ #ifndef LAND_DETECTOR_TRIGGER # define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. #endif +#ifndef LAND_DETECTOR_MAYBE_TRIGGER + # define LAND_DETECTOR_MAYBE_TRIGGER 10 // number of 50hz iterations with near zero climb rate and low throttle that means we might be landed (used to reset horizontal position targets to prevent tipping over) +#endif #ifndef LAND_DETECTOR_CLIMBRATE_MAX # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s #endif diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index e4dddc22ead..f34692f20b9 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -287,6 +287,11 @@ static void auto_land_run() return; } + // init loiter targets when maybe landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 237aad939d8..7299e8fe581 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -73,6 +73,11 @@ static void land_gps_run() return; } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { @@ -187,6 +192,11 @@ static float get_throttle_land() } } +static bool land_maybe_complete() +{ + return (ap.land_complete || land_detector > LAND_DETECTOR_MAYBE_TRIGGER); +} + // update_land_detector - checks if we have landed and updates the ap.land_complete flag // called at 50hz static void update_land_detector() diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 3883342e6f6..6240290a7b2 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -68,6 +68,11 @@ static void loiter_run() wp_nav.clear_pilot_desired_acceleration(); } + // relax loiter target if we might be landed + if (land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // when landed reset targets and output zero throttle if (ap.land_complete) { wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index beec114b5bd..f9d1c7f4c01 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -183,6 +183,11 @@ static void poshold_run() } } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // if landed initialise loiter targets, set throttle to zero and exit if (ap.land_complete) { wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 2caafe4e924..f0ece2b864f 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -349,6 +349,11 @@ static void rtl_land_run() return; } + // relax loiter target if we might be landed + if(land_maybe_complete()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { From 5f909f1a73d73172008135cc95ae575509b42f49 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Sep 2014 15:54:36 +0900 Subject: [PATCH 088/254] Copter: add land_complete_maybe flag --- ArduCopter/AP_State.pde | 15 +++++++++++++++ ArduCopter/ArduCopter.pde | 1 + ArduCopter/control_land.pde | 10 +++++++--- ArduCopter/defines.h | 1 + ArduCopter/motors.pde | 1 + ArduCopter/system.pde | 1 + 6 files changed, 26 insertions(+), 3 deletions(-) diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 63ccaa975ea..fcba1c36c33 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -110,6 +110,21 @@ void set_land_complete(bool b) // --------------------------------------------- +// set land complete maybe flag +void set_land_complete_maybe(bool b) +{ + // if no change, exit immediately + if (ap.land_complete_maybe == b) + return; + + if (b) { + Log_Write_Event(DATA_LAND_COMPLETE_MAYBE); + } + ap.land_complete_maybe = b; +} + +// --------------------------------------------- + void set_pre_arm_check(bool b) { if(ap.pre_arm_check != b) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ddec25f849e..0758c275e08 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -389,6 +389,7 @@ static union { uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 18 // true if we may have landed (less strict version of land_complete) }; uint32_t value; } ap; diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 7299e8fe581..14399be58c8 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // counter to verify landings -static uint16_t land_detector; +static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed static bool land_with_gps; static uint32_t land_start_time; @@ -192,9 +192,10 @@ static float get_throttle_land() } } +// land_maybe_complete - return true if we may have landed (used to reset loiter targets during landing) static bool land_maybe_complete() { - return (ap.land_complete || land_detector > LAND_DETECTOR_MAYBE_TRIGGER); + return (ap.land_complete || ap.land_complete_maybe); } // update_land_detector - checks if we have landed and updates the ap.land_complete flag @@ -210,7 +211,7 @@ static void update_land_detector() land_detector++; }else{ set_land_complete(true); - land_detector = 0; + land_detector = LAND_DETECTOR_TRIGGER; } } } else { @@ -221,6 +222,9 @@ static void update_land_detector() set_land_complete(false); } } + + // set land maybe flag + set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER); } // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 51269ab16be..e6f4f197aa8 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -275,6 +275,7 @@ enum FlipState { #define DATA_DISARMED 11 #define DATA_AUTO_ARMED 15 #define DATA_TAKEOFF 16 +#define DATA_LAND_COMPLETE_MAYBE 17 #define DATA_LAND_COMPLETE 18 #define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 65af49887a7..68ab86d9353 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -615,6 +615,7 @@ static void init_disarm_motors() // we are not in the air set_land_complete(true); + set_land_complete_maybe(true); // reset the mission mission.reset(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e0fa98560df..0f2cba66f2a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -325,6 +325,7 @@ static void startup_ground(bool force_gyro_cal) // set landed flag set_land_complete(true); + set_land_complete_maybe(true); } // returns true if the GPS is ok and home position is set From 93dbfd009e1468358afdc763588ec2ef10ccb6c4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Sep 2014 16:28:11 +0900 Subject: [PATCH 089/254] Copter: rename land_maybe_complete function --- ArduCopter/control_auto.pde | 4 ++-- ArduCopter/control_land.pde | 6 +++--- ArduCopter/control_loiter.pde | 2 +- ArduCopter/control_poshold.pde | 2 +- ArduCopter/control_rtl.pde | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index f34692f20b9..1a7a55f437b 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -287,8 +287,8 @@ static void auto_land_run() return; } - // init loiter targets when maybe landed - if(land_maybe_complete()) { + // relax loiter targets if we might be landed + if (land_complete_maybe()) { wp_nav.loiter_soften_for_landing(); } diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 14399be58c8..1092df79f90 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -74,7 +74,7 @@ static void land_gps_run() } // relax loiter target if we might be landed - if(land_maybe_complete()) { + if (land_complete_maybe()) { wp_nav.loiter_soften_for_landing(); } @@ -192,8 +192,8 @@ static float get_throttle_land() } } -// land_maybe_complete - return true if we may have landed (used to reset loiter targets during landing) -static bool land_maybe_complete() +// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing) +static bool land_complete_maybe() { return (ap.land_complete || ap.land_complete_maybe); } diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 6240290a7b2..f606636adc9 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -69,7 +69,7 @@ static void loiter_run() } // relax loiter target if we might be landed - if (land_maybe_complete()) { + if (land_complete_maybe()) { wp_nav.loiter_soften_for_landing(); } diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index f9d1c7f4c01..562f5e72738 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -184,7 +184,7 @@ static void poshold_run() } // relax loiter target if we might be landed - if(land_maybe_complete()) { + if (land_complete_maybe()) { wp_nav.loiter_soften_for_landing(); } diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index f0ece2b864f..28ce1387851 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -350,7 +350,7 @@ static void rtl_land_run() } // relax loiter target if we might be landed - if(land_maybe_complete()) { + if (land_complete_maybe()) { wp_nav.loiter_soften_for_landing(); } From 1cf7f7eaa98d27e515ff54996e3e8cd77295735c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Sep 2014 12:24:36 +0900 Subject: [PATCH 090/254] Copter: increase Alt Disparity check to 2m --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 60475602062..9d6f2ca3481 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -301,7 +301,7 @@ // pre-arm baro vs inertial nav max alt disparity #ifndef PREARM_MAX_ALT_DISPARITY_CM - # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters + # define PREARM_MAX_ALT_DISPARITY_CM 200 // barometer and inertial nav altitude must be within this many centimeters #endif // pre-arm check max velocity From 9693ee0417a0c8efb986a7588414a6b153cc0e66 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 21 Sep 2014 17:33:59 +0900 Subject: [PATCH 091/254] LowPassFilter: add div by zero check --- libraries/Filter/LowPassFilter.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h index fbab2c52d6d..515e88179d0 100644 --- a/libraries/Filter/LowPassFilter.h +++ b/libraries/Filter/LowPassFilter.h @@ -86,6 +86,12 @@ LowPassFilter::LowPassFilter() : template void LowPassFilter::set_cutoff_frequency(float time_step, float cutoff_freq) { + // avoid divide by zero and allow removing filtering + if (cutoff_freq <= 0.0f) { + _alpha = 1.0f; + return; + } + // calculate alpha float rc = 1/(2*PI*cutoff_freq); _alpha = time_step / (time_step + rc); @@ -94,6 +100,12 @@ void LowPassFilter::set_cutoff_frequency(float time_step, float cutoff_freq) template void LowPassFilter::set_time_constant(float time_step, float time_constant) { + // avoid divide by zero + if (time_constant + time_step <= 0.0f) { + _alpha = 1.0f; + return; + } + // calculate alpha _alpha = time_step / (time_constant + time_step); } From e04709301395527c71db49c03a6510f2ccf34161 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 21 Sep 2014 17:52:14 +0900 Subject: [PATCH 092/254] AC_PosControl: 2hz filter on accel error Replaced hard-coded filter with LowPassFilter class allowing the filter's to be 2hz on both APM and Pixhawk --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 +++++-- libraries/AC_AttitudeControl/AC_PosControl.h | 3 +++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 34b066cb7e6..0734f578271 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -84,6 +84,9 @@ void AC_PosControl::set_dt(float delta_sec) // update rate controller's d filter _pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt); + + // update rate z-axis velocity error and accel error filters + _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ); } /// set_speed_z - sets maximum climb and descent rates @@ -365,11 +368,11 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) if (_flags.reset_accel_to_throttle) { // Reset Filter _accel_error.z = 0; + _accel_error_filter.reset(0); _flags.reset_accel_to_throttle = false; } else { // calculate accel error and Filter with fc = 2 Hz - // To-Do: replace constant below with one that is adjusted for update rate - _accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z); + _accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000)); } // separately calculate p, i, d values for logging diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 14ce41faf1a..71ca7294683 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -40,6 +40,8 @@ #define POSCONTROL_VEL_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple) +#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error + class AC_PosControl { public: @@ -367,6 +369,7 @@ class AC_PosControl float _distance_to_target; // distance to position target - for reporting only uint8_t _xy_step; // used to decide which portion of horizontal position controller to run during this iteration float _dt_xy; // time difference in seconds between horizontal position updates + LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error // velocity controller internal variables uint8_t _vel_xyz_step; // used to decide which portion of velocity controller to run during this iteration From 5e197407b910e74de44a616335f17235d94eb5ff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 21 Sep 2014 17:53:10 +0900 Subject: [PATCH 093/254] AC_PosControl: 4hz filter on z-axis velocity error --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 5 ++++- libraries/AC_AttitudeControl/AC_PosControl.h | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0734f578271..77cc51e784d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -86,6 +86,7 @@ void AC_PosControl::set_dt(float delta_sec) _pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt); // update rate z-axis velocity error and accel error filters + _vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ); _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ); } @@ -337,10 +338,12 @@ void AC_PosControl::rate_to_accel_z() if (_flags.reset_rate_to_accel_z) { // Reset Filter _vel_error.z = 0; + _vel_error_filter.reset(0); desired_accel = 0; _flags.reset_rate_to_accel_z = false; } else { - _vel_error.z = (_vel_target.z - curr_vel.z); + // calculate rate error and filter with cut off frequency of 2 Hz + _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z); } // calculate p diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 71ca7294683..52ffaa02e72 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -40,6 +40,7 @@ #define POSCONTROL_VEL_UPDATE_TIME 0.020f // 50hz update rate on high speed CPUs (Pixhawk, Flymaple) +#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0 // 4hz low-pass filter on velocity error #define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0 // 2hz low-pass filter on accel error class AC_PosControl @@ -369,6 +370,7 @@ class AC_PosControl float _distance_to_target; // distance to position target - for reporting only uint8_t _xy_step; // used to decide which portion of horizontal position controller to run during this iteration float _dt_xy; // time difference in seconds between horizontal position updates + LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error // velocity controller internal variables From d66ffd5b07431e20625ebd118c98460ae424aef3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Sep 2014 19:33:04 +0900 Subject: [PATCH 094/254] Copter: use disparity threshold define for pre-arm checks There are two duplicate checks, one in the pre-arm checks (i.e. checks run every 15 seconds or so before the vehicle is armed) and one in the arming checks (run immediately before arming). The definition in the pre-arm checks was still using the old hardcoded value. --- ArduCopter/motors.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 68ab86d9353..342ae0362a2 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -253,7 +253,7 @@ static void pre_arm_checks(bool display_failure) return; } // check Baro & inav alt are within 1m - if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) { + if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Alt disparity")); } From 9c3379ff48babead9f3ce2e97ebbb73fbc39ce0c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Sep 2014 20:35:18 +0900 Subject: [PATCH 095/254] Compass: always default devid to zero --- libraries/AP_Compass/Compass.cpp | 6 +++--- libraries/AP_Compass/Compass.h | 19 ------------------- 2 files changed, 3 insertions(+), 22 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 02920acc1a5..0df6cc0f3c9 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -120,13 +120,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // @DisplayName: Compass device id // @Description: Compass device id. Automatically detected, do not set manually // @User: Advanced - AP_GROUPINFO("DEV_ID", 15, Compass, _dev_id[0], COMPASS_EXPECTED_DEV_ID), + AP_GROUPINFO("DEV_ID", 15, Compass, _dev_id[0], 0), // @Param: DEV_ID2 // @DisplayName: Compass2 device id // @Description: Second compass's device id. Automatically detected, do not set manually // @User: Advanced - AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], COMPASS_EXPECTED_DEV_ID2), + AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], 0), #endif #if COMPASS_MAX_INSTANCES > 2 @@ -134,7 +134,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // @DisplayName: Compass3 device id // @Description: Third compass's device id. Automatically detected, do not set manually // @User: Advanced - AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], COMPASS_EXPECTED_DEV_ID3), + AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], 0), #endif AP_GROUPEND diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index ae115dcaa98..71b3fdb4b53 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -52,25 +52,6 @@ #define COMPASS_MAX_INSTANCES 1 #endif -// default compass device ids for each board type to most common set-up to reduce eeprom usage -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -# define COMPASS_EXPECTED_DEV_ID 73225 // external hmc5883 -# define COMPASS_EXPECTED_DEV_ID2 -1 // internal ldm303d -# define COMPASS_EXPECTED_DEV_ID3 0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX -# define COMPASS_EXPECTED_DEV_ID 0 -# define COMPASS_EXPECTED_DEV_ID2 0 -# define COMPASS_EXPECTED_DEV_ID3 0 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -# define COMPASS_EXPECTED_DEV_ID 0 -# define COMPASS_EXPECTED_DEV_ID2 0 -# define COMPASS_EXPECTED_DEV_ID3 0 -#else -# define COMPASS_EXPECTED_DEV_ID 0 -# define COMPASS_EXPECTED_DEV_ID2 0 -# define COMPASS_EXPECTED_DEV_ID3 0 -#endif - class Compass { public: From 62339c217c4c2743cf2b4b466d767bb11b6655ec Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 23 Sep 2014 19:32:20 +1000 Subject: [PATCH 096/254] AP_AHRS : Prevent EKF starting if GPS sats less than AHRS_GPS_MINSATS --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 14b1d0a2769..71ff44b17a4 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -58,8 +58,8 @@ void AP_AHRS_NavEKF::update(void) _dcm_attitude(roll, pitch, yaw); if (!ekf_started) { - // if we have a GPS lock we can start the EKF - if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { + // if we have a GPS lock and more than 6 satellites, we can start the EKF + if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && get_gps().num_sats() >= _gps_minsats) { if (start_time_ms == 0) { start_time_ms = hal.scheduler->millis(); } From 3233416e437a1c7aa5b0c65221c2401e337ce28f Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 23 Sep 2014 19:30:01 +1000 Subject: [PATCH 097/254] AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++++++++++- libraries/AP_NavEKF/AP_NavEKF.h | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a0742a0ac9a..9b43a7ce326 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1746,7 +1746,7 @@ void NavEKF::FuseVelPosNED() velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2 // calculate innovation variance varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - // calculate error weightings for singloe IMU velocity states using + // calculate error weightings for single IMU velocity states using // observation error to normalise float R_hgt; if (i == 2) { @@ -1839,12 +1839,15 @@ void NavEKF::FuseVelPosNED() if (fuseData[obsIndex]) { stateIndex = 4 + obsIndex; // calculate the measurement innovation, using states from a different time coordinate if fusing height data + // adjust scaling on GPS measurement noise variances if not enough satellites if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex]; + R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 3 || obsIndex == 4) { innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex]; + R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else { innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex]; } @@ -3102,6 +3105,15 @@ void NavEKF::readGpsData() // read the NED velocity from the GPS velNED = _ahrs->get_gps().velocity(); + // check if we have enough GPS satellites and increase the gps noise scaler if we don't + if (_ahrs->get_gps().num_sats() >= 6) { + gpsNoiseScaler = 1.0f; + } else if (_ahrs->get_gps().num_sats() == 5) { + gpsNoiseScaler = 1.4f; + } else if (_ahrs->get_gps().num_sats() <= 4) { + gpsNoiseScaler = 2.0f; + } + // Check if GPS can output vertical velocity and set GPS fusion mode accordingly if (!_ahrs->get_gps().have_vertical_velocity()) { // vertical velocity should not be fused diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 5e7e68bc2ed..ba27b1bfbe9 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -364,6 +364,7 @@ class NavEKF bool filterDiverged; // boolean true if the filter has diverged bool magFailed; // boolean true if the magnetometer has failed + float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts Vector31 Kfusion; // Kalman gain vector Matrix22 KH; // intermediate result used for covariance updates Matrix22 KHP; // intermediate result used for covariance updates From a0cb4301a1e4ee5c585da27947528dea2d438f6b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Sep 2014 22:55:19 +0900 Subject: [PATCH 098/254] Copter: allow passthru for ch 9 ~ 14 Based on work by Emile Castelnuovo --- ArduCopter/radio.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index d3914d34ffb..f4853f3bf17 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -114,6 +114,13 @@ static void read_radio() g.rc_7.set_pwm(periods[6]); g.rc_8.set_pwm(periods[7]); + // read channels 9 ~ 14 + for (uint8_t i=8; iset_pwm(RC_Channel::rc_channel(i)->read()); + } + } + // flag we must have an rc receiver attached if (!failsafe.rc_override_active) { ap.rc_receiver_present = true; From 9c1b4e2332732aad330e603b196e00cd4f78cbe0 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 24 Sep 2014 14:19:55 +1000 Subject: [PATCH 099/254] AP_NavEKF : Explicitly initialise gpsNoiseScaler to default value --- libraries/AP_NavEKF/AP_NavEKF.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 9b43a7ce326..9614bf3dd6c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3385,6 +3385,7 @@ void NavEKF::ZeroVariables() secondLastFixTime_ms = imuSampleTime_ms; lastDecayTime_ms = imuSampleTime_ms; + gpsNoiseScaler = 1.0f; velTimeout = false; posTimeout = false; hgtTimeout = false; From 56242176dacc6d0425d89cb16e598ec10be21100 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Sep 2014 14:02:35 +0900 Subject: [PATCH 100/254] Copter: update AC3.2-rc10 release notes --- ArduCopter/ReleaseNotes.txt | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 6a2199ca367..fad3b832870 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,18 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc10 24-Sep-2014 +Changes from 3.2-rc9 +1) two-stage land-detector to reduce motor run-up when landing in Loiter, PosHold, RTL, Auto +2) Allow passthrough from input to output of channels 9 ~ 14 (thanks Emile!) +3) Add 4hz filter to vertical velocity error during AltHold +4) Safety Feature: + a) increase Alt Disparity pre-arm check threshold to 2m (was 1m) + b) reset battery failsafe after disarming/arming (thanks AndKe!) + c) EKF only apply centrifugal corrections when GPS has at least 6 satellites (Pixhawk with EKF enabled only) +5) Bug fixes: + a) to default compass devid to zero when no compass connected + b) reduce motor run-up while landing in RTL +------------------------------------------------------------------ ArduCopter 3.2-rc9 11-Sep-2014 Changes from 3.2-rc8 1) FRAM bug fix that could stop Mission or Parameter changes from being saved (Pixhawk, VRBrain only) From eb89b537142c5ad62a02ddde8af5a90c97b6ca3c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Sep 2014 14:02:50 +0900 Subject: [PATCH 101/254] Copter: version to AC3.2-rc10 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0758c275e08..e7003686067 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-Iris" +#define THISFIRMWARE "ArduCopter V3.2-rc10" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 20d35b4bd13d1e04c0f5d333e0ca697bbdfd83b6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Sep 2014 12:20:40 +0900 Subject: [PATCH 102/254] Copter: bugfix to condition-yaw for relative angles Thanks to roque-canales for raising the issue and paradisephil for finding the specific piece of code that went wrong and suggesting the fix. --- ArduCopter/GCS_Mavlink.pde | 4 ++-- ArduCopter/commands_logic.pde | 1 + ArduCopter/control_auto.pde | 7 +++++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 9c6f3d30468..6dc344785d6 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1057,12 +1057,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] // param2 : speed during change [deg per second] - // param3 : direction (not supported) + // param3 : direction (-1:ccw, +1:cw) // param4 : relative offset (1) or absolute angle (0) if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && ((packet.param4 == 0) || (packet.param4 == 1))) { - set_auto_yaw_look_at_heading(packet.param1, packet.param2, (uint8_t)packet.param4); + set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index d2410637f7b..3d5968c097b 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -782,6 +782,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd) set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, + cmd.content.yaw.direction, cmd.content.yaw.relative_angle); } diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 1a7a55f437b..ce7e744b867 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -475,7 +475,7 @@ void set_auto_yaw_mode(uint8_t yaw_mode) } // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, uint8_t relative_angle) +static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; @@ -486,7 +486,10 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle - yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + if (direction < 0) { + angle_deg = -angle_deg; + } + yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed From 1f1670279b9c640b08440565c96905328c8750ff Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 17 Sep 2014 04:52:23 +1000 Subject: [PATCH 103/254] AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 46 ++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 9614bf3dd6c..8d57c0bdd18 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -407,12 +407,17 @@ void NavEKF::ResetPosition(void) // read the GPS readGpsData(); - // write to state vector - states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x; - states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y; + // write to state vector and compensate for GPS latency + states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); + states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); } // reset the glitch ofset correction states gpsPosGlitchOffsetNE.zero(); + // stored horizontal position states to prevent subsequent GPS measurements from being rejected + for (uint8_t i=0; i<=49; i++){ + storedStates[i].position[0] = state.position[0]; + storedStates[i].position[1] = state.position[1]; + } } // resets velocity states to last GPS measurement or to zero if in static mode @@ -425,20 +430,25 @@ void NavEKF::ResetVelocity(void) } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // read the GPS readGpsData(); - // reset horizontal velocity states - if (_fusionModeGPS <= 1) { - states[4] = velNED[0]; // north velocity from blended accel data - states[5] = velNED[1]; // east velocity from blended accel data - states[23] = velNED[0]; // north velocity from IMU1 accel data - states[24] = velNED[1]; // east velocity from IMU1 accel data - states[27] = velNED[0]; // north velocity from IMU2 accel data - states[28] = velNED[1]; // east velocity from IMU2 accel data + // Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement) + if (_fusionModeGPS >= 1) { + velNED[2] = 0.0f; } - // reset vertical velocity states - if (_fusionModeGPS <= 0) { - states[6] = velNED[2]; // down velocity from blended accel data - states[25] = velNED[2]; // down velocity from IMU1 accel data - states[29] = velNED[2]; // down velocity from IMU2 accel data + // reset filter velocity states + states[4] = velNED[0]; // north velocity from blended accel data + states[5] = velNED[1]; // east velocity from blended accel data + states[23] = velNED[0]; // north velocity from IMU1 accel data + states[24] = velNED[1]; // east velocity from IMU1 accel data + states[27] = velNED[0]; // north velocity from IMU2 accel data + states[28] = velNED[1]; // east velocity from IMU2 accel data + states[6] = velNED[2]; // down velocity from blended accel data + states[25] = velNED[2]; // down velocity from IMU1 accel data + states[29] = velNED[2]; // down velocity from IMU2 accel data + // reset stored velocity states to prevent subsequent GPS measurements from being rejected + for (uint8_t i=0; i<=49; i++){ + storedStates[i].velocity[0] = velNED[0]; + storedStates[i].velocity[1] = velNED[1]; + storedStates[i].velocity[2] = velNED[2]; } } } @@ -452,6 +462,10 @@ void NavEKF::ResetHeight(void) states[9] = -hgtMea; // down position from blended accel data state.posD1 = -hgtMea; // down position from IMU1 accel data state.posD2 = -hgtMea; // down position from IMU2 accel data + // reset stored vertical position states to prevent subsequent GPS measurements from being rejected + for (uint8_t i=0; i<=49; i++){ + storedStates[i].position[2] = -hgtMea; + } } // this function is used to initialise the filter whilst moving, using the AHRS DCM solution From a6cd04ca4cc33b5837c86a584e3ba76051bb6f15 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 7 Sep 2014 09:14:13 +1000 Subject: [PATCH 104/254] AP_NavEKF : Fix bug in GPS innovation variance growth calculation The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error. The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres. This error was found during flight testing with 8g pullups --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8d57c0bdd18..52b15bc28f4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1714,7 +1714,7 @@ void NavEKF::FuseVelPosNED() // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring - float accelScale = (1.0f + 0.1f * accNavMagHoriz); + float accelScale = (1.0f + 0.1f * accNavMag); float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); From 2d7f819e1d1505e91a507441459efd29c8abed04 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Sep 2014 15:26:18 +0900 Subject: [PATCH 105/254] AC_WPNav: add shift_wp_origin_to_current_pos for takeoff This shifts the origin to the vehicle's current position and should be called just before take-off to ensure there are no sudden roll or pitch moves on takeoff. --- libraries/AC_WPNav/AC_WPNav.cpp | 27 +++++++++++++++++++++++++++ libraries/AC_WPNav/AC_WPNav.h | 5 +++++ 2 files changed, 32 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 1fa531be4a4..3fd32c0d0d1 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -414,6 +414,33 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms); } +/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position +/// used to reset the position just before takeoff +/// relies on set_wp_destination or set_wp_origin_and_destination having been called first +void AC_WPNav::shift_wp_origin_to_current_pos() +{ + // return immediately if vehicle is not at the origin + if (_track_desired > 0.0f) { + return; + } + + // get current and target locations + const Vector3f curr_pos = _inav.get_position(); + const Vector3f pos_target = _pos_control.get_pos_target(); + + // calculate difference between current position and target + Vector3f pos_diff = curr_pos - pos_target; + + // shift origin and destination + _origin += pos_diff; + _destination += pos_diff; + + // move pos controller target and disable feed forward + _pos_control.set_pos_target(curr_pos); + _pos_control.freeze_ff_xy(); + _pos_control.freeze_ff_z(); +} + /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 985f1206fae..60bcd095af3 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -135,6 +135,11 @@ class AC_WPNav /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination); + /// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position + /// used to reset the position just before takeoff + /// relies on set_wp_destination or set_wp_origin_and_destination having been called first + void shift_wp_origin_to_current_pos(); + /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration /// results placed in stopping_position vector void get_wp_stopping_point_xy(Vector3f& stopping_point) const; From da2a463f18eedac351d4ec9a53eea44857079410 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Sep 2014 15:26:54 +0900 Subject: [PATCH 106/254] Copter: shift pos targets to current location before takeoff --- ArduCopter/control_auto.pde | 3 ++- ArduCopter/control_guided.pde | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index ce7e744b867..6648f46bbce 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -102,13 +102,14 @@ static void auto_takeoff_run() { // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { + // initialise wpnav targets + wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // tell motors to do a slow start motors.slow_start(true); - // To-Do: re-initialise wpnav targets return; } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 30f5fd9f176..02deb9d4619 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -140,13 +140,14 @@ static void guided_takeoff_run() { // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { + // initialise wpnav targets + wp_nav.shift_wp_origin_to_current_pos(); // reset attitude control targets attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // tell motors to do a slow start motors.slow_start(true); - // To-Do: re-initialise wpnav targets return; } From c338b1675f9a1e7ec011353d24a1f7c4c5b6919a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Oct 2014 11:41:56 +0900 Subject: [PATCH 107/254] GPS: fix SIRF set-binary message This fixes an issue in which the the update rate for the mediatek, which uses a similar protocol, was not being set correctly --- libraries/AP_GPS/AP_GPS_SIRF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index c69176a02d2..15d1ece9bf3 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -25,7 +25,7 @@ #include #include -#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" +#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C\r\n" class AP_GPS_SIRF : public AP_GPS_Backend { public: From 468ebd811c18aaf9fbf2506d6d93bd1ab5ad4e4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Oct 2014 12:50:53 +1000 Subject: [PATCH 108/254] AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 145 ++++++++++++------------------ 1 file changed, 56 insertions(+), 89 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 52b15bc28f4..c2ce80d6019 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -401,15 +401,15 @@ bool NavEKF::PositionDrifting(void) const void NavEKF::ResetPosition(void) { if (staticMode) { - states[7] = 0; - states[8] = 0; + state.position.x = 0; + state.position.y = 0; } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { // read the GPS readGpsData(); // write to state vector and compensate for GPS latency - states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); - states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); + state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); + state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); } // reset the glitch ofset correction states gpsPosGlitchOffsetNE.zero(); @@ -435,20 +435,12 @@ void NavEKF::ResetVelocity(void) velNED[2] = 0.0f; } // reset filter velocity states - states[4] = velNED[0]; // north velocity from blended accel data - states[5] = velNED[1]; // east velocity from blended accel data - states[23] = velNED[0]; // north velocity from IMU1 accel data - states[24] = velNED[1]; // east velocity from IMU1 accel data - states[27] = velNED[0]; // north velocity from IMU2 accel data - states[28] = velNED[1]; // east velocity from IMU2 accel data - states[6] = velNED[2]; // down velocity from blended accel data - states[25] = velNED[2]; // down velocity from IMU1 accel data - states[29] = velNED[2]; // down velocity from IMU2 accel data + state.velocity = velNED; + state.vel1 = velNED; + state.vel2 = velNED; // reset stored velocity states to prevent subsequent GPS measurements from being rejected for (uint8_t i=0; i<=49; i++){ - storedStates[i].velocity[0] = velNED[0]; - storedStates[i].velocity[1] = velNED[1]; - storedStates[i].velocity[2] = velNED[2]; + storedStates[i].velocity = velNED; } } } @@ -459,12 +451,12 @@ void NavEKF::ResetHeight(void) // read the altimeter readHgtData(); // write to the state vector - states[9] = -hgtMea; // down position from blended accel data + state.position.z = -hgtMea; // down position from blended accel data state.posD1 = -hgtMea; // down position from IMU1 accel data state.posD2 = -hgtMea; // down position from IMU2 accel data // reset stored vertical position states to prevent subsequent GPS measurements from being rejected for (uint8_t i=0; i<=49; i++){ - storedStates[i].position[2] = -hgtMea; + storedStates[i].position.z = -hgtMea; } } @@ -491,11 +483,9 @@ void NavEKF::InitialiseFilterDynamic(void) magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); // calculate initial orientation and earth magnetic field states - Quaternion initQuat; - initQuat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); + state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch); // write to state vector - state.quat = initQuat; state.gyro_bias.zero(); state.accel_zbias1 = 0; state.accel_zbias2 = 0; @@ -909,9 +899,8 @@ void NavEKF::UpdateStrapdownEquationsNED() state.quat = qUpdated; // calculate the body to nav cosine matrix - Quaternion q(states[0],states[1],states[2],states[3]); Matrix3f Tbn_temp; - q.rotation_matrix(Tbn_temp); + state.quat.rotation_matrix(Tbn_temp); prevTnb = Tbn_temp.transposed(); // transform body delta velocities to delta velocities in the nav frame @@ -987,7 +976,7 @@ void NavEKF::CovariancePrediction() // this allows for wind gradient effects. // filter height rate using a 10 second time constant filter float alpha = 0.1f * dt; - hgtRate = hgtRate * (1.0f - alpha) - states[6] * alpha; + hgtRate = hgtRate * (1.0f - alpha) - state.velocity.z * alpha; // use filtered height rate to increase wind process noise when climbing or descending // this allows for wind gradient effects. @@ -1027,14 +1016,14 @@ void NavEKF::CovariancePrediction() dax = summedDelAng.x; day = summedDelAng.y; daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - dvz_b = IMU1_weighting * states[13] + (1.0f - IMU1_weighting) * states[22]; + q0 = state.quat[0]; + q1 = state.quat[1]; + q2 = state.quat[2]; + q3 = state.quat[3]; + dax_b = state.gyro_bias.x; + day_b = state.gyro_bias.y; + daz_b = state.gyro_bias.z; + dvz_b = IMU1_weighting * state.accel_zbias1 + (1.0f - IMU1_weighting) * state.accel_zbias2; _gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f); daxCov = sq(dt*_gyrNoise); dayCov = sq(dt*_gyrNoise); @@ -1914,8 +1903,8 @@ void NavEKF::FuseVelPosNED() float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex]; // Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3 float correctionLimit = 0.02f * dtIMU *dtVelPos; - states[13] = states[13] - constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias - states[22] = states[22] - constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias + state.accel_zbias1 -= constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias + state.accel_zbias2 -= constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias for (uint8_t i = 23; i<=26; i++) { states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD } @@ -2472,11 +2461,8 @@ void NavEKF::FuseAirspeed() states[j] = states[j] - Kfusion[j] * innovVtas; } - Quaternion q(states[0], states[1], states[2], states[3]); - q.normalize(); - for (uint8_t i = 0; i<=3; i++) { - states[i] = q[i]; - } + state.quat.normalize(); + // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the number of operations for (uint8_t i = 0; i<=21; i++) @@ -2550,15 +2536,15 @@ void NavEKF::FuseSideslip() float innovBeta; // copy required states to local variable names - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - vn = states[4]; - ve = states[5]; - vd = states[6]; - vwn = states[14]; - vwe = states[15]; + q0 = state.quat[0]; + q1 = state.quat[1]; + q2 = state.quat[2]; + q3 = state.quat[3]; + vn = state.velocity.x; + ve = state.velocity.y; + vd = state.velocity.z; + vwn = state.wind_vel.x; + vwe = state.wind_vel.y; // calculate predicted wind relative velocity in NED vel_rel_wind.x = vn - vwn; @@ -2666,11 +2652,8 @@ void NavEKF::FuseSideslip() states[j] = states[j] - Kfusion[j] * innovBeta; } - Quaternion q(states[0], states[1], states[2], states[3]); - q.normalize(); - for (uint8_t i = 0; i<=3; i++) { - states[i] = q[i]; - } + state.quat.normalize(); + // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations @@ -2802,26 +2785,21 @@ void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const // return the Euler roll, pitch and yaw angle in radians void NavEKF::getEulerAngles(Vector3f &euler) const { - Quaternion q(states[0], states[1], states[2], states[3]); - q.to_euler(&euler.x, &euler.y, &euler.z); + state.quat.to_euler(&euler.x, &euler.y, &euler.z); euler = euler - _ahrs->get_trim(); } // return NED velocity in m/s void NavEKF::getVelNED(Vector3f &vel) const { - vel.x = states[4]; - vel.y = states[5]; - vel.z = states[6]; + vel = state.velocity; } // return the last calculated NED position relative to the reference point (m). // return false if no position is available bool NavEKF::getPosNED(Vector3f &pos) const { - pos.x = states[7]; - pos.y = states[8]; - pos.z = states[9]; + pos = state.position; return true; } @@ -2832,9 +2810,7 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const gyroBias.zero(); return; } - gyroBias.x = states[10] / dtIMU; - gyroBias.y = states[11] / dtIMU; - gyroBias.z = states[12] / dtIMU; + gyroBias = state.gyro_bias / dtIMU; } // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 @@ -2845,33 +2821,29 @@ void NavEKF::getAccelBias(Vector3f &accelBias) const accelBias.y = 0; accelBias.z = 0; } else { - accelBias.y = states[22] / dtIMU; - accelBias.z = states[13] / dtIMU; + accelBias.y = state.accel_zbias2 / dtIMU; + accelBias.z = state.accel_zbias1 / dtIMU; } } // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) void NavEKF::getWind(Vector3f &wind) const { - wind.x = states[14]; - wind.y = states[15]; + wind.x = state.wind_vel.x; + wind.y = state.wind_vel.y; wind.z = 0.0f; // currently don't estimate this } // return earth magnetic field estimates in measurement units / 1000 void NavEKF::getMagNED(Vector3f &magNED) const { - magNED.x = states[16]*1000.0f; - magNED.y = states[17]*1000.0f; - magNED.z = states[18]*1000.0f; + magNED = state.earth_magfield * 1000.0f; } // return body magnetic field estimates in measurement units / 1000 void NavEKF::getMagXYZ(Vector3f &magXYZ) const { - magXYZ.x = states[19]*1000.0f; - magXYZ.y = states[20]*1000.0f; - magXYZ.z = states[21]*1000.0f; + magXYZ = state.body_magfield*1000.0f; } // return the last calculated latitude, longitude and height @@ -2879,10 +2851,10 @@ bool NavEKF::getLLH(struct Location &loc) const { loc.lat = _ahrs->get_home().lat; loc.lng = _ahrs->get_home().lng; - loc.alt = _ahrs->get_home().alt - states[9]*100; + loc.alt = _ahrs->get_home().alt - state.position.z*100; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; - location_offset(loc, states[7], states[8]); + location_offset(loc, state.position.x, state.position.y); return true; } @@ -3278,24 +3250,20 @@ void NavEKF::alignYawGPS() float newYaw; float yawErr; // get quaternion from existing filter states and calculate roll, pitch and yaw angles - Quaternion initQuat; - Quaternion newQuat; - for (uint8_t i=0; i<=3; i++) initQuat[i] = states[i]; - initQuat.to_euler(&roll, &pitch, &oldYaw); + state.quat.to_euler(&roll, &pitch, &oldYaw); // calculate yaw angle from GPS velocity newYaw = atan2f(velNED[1],velNED[0]); // modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned yawErr = fabsf(newYaw - oldYaw); if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) { // calculate new filter quaternion states from Euler angles - newQuat.from_euler(roll, pitch, newYaw); - for (uint8_t i=0; i<=3; i++) states[i] = newQuat[i]; + state.quat.from_euler(roll, pitch, newYaw); // the yaw angle is now aligned so update its status yawAligned = true; // set the velocity states if (_fusionModeGPS < 2) { - states[4] = velNED[0]; - states[5] = velNED[1]; + state.velocity.x = velNED.x; + state.velocity.y = velNED.y; } // reinitialise the quaternion, velocity and position covariances // zero the matrix entries @@ -3326,12 +3294,12 @@ void NavEKF::alignYawGPS() // representative of typical launch wind void NavEKF::setWindVelStates() { - float gndSpd = sqrtf(sq(states[4]) + sq(states[5])); + float gndSpd = pythagorous2(state.velocity.x, state.velocity.y); if (gndSpd > 4.0f) { // set the wind states to be the reciprocal of the velocity and scale float scaleFactor = STARTUP_WIND_SPEED / gndSpd; - states[14] = - states[4] * scaleFactor; - states[15] = - states[5] * scaleFactor; + state.wind_vel.x = - state.velocity.x * scaleFactor; + state.wind_vel.y = - state.velocity.y * scaleFactor; // reinitialise the wind state covariances zeroRows(P,14,15); zeroCols(P,14,15); @@ -3344,8 +3312,7 @@ void NavEKF::setWindVelStates() void NavEKF::getRotationBodyToNED(Matrix3f &mat) const { Vector3f trim = _ahrs->get_trim(); - Quaternion q(states[0], states[1], states[2], states[3]); - q.rotation_matrix(mat); + state.quat.rotation_matrix(mat); mat.rotateXYinv(trim); } From 89755c2bb5e18738741b190d6f66aea6263719d2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Oct 2014 12:54:27 +1000 Subject: [PATCH 109/254] AP_NavEKF: make it clear that all sat counts are covered --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index c2ce80d6019..42743169268 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3096,7 +3096,7 @@ void NavEKF::readGpsData() gpsNoiseScaler = 1.0f; } else if (_ahrs->get_gps().num_sats() == 5) { gpsNoiseScaler = 1.4f; - } else if (_ahrs->get_gps().num_sats() <= 4) { + } else { // <= 4 satellites gpsNoiseScaler = 2.0f; } From b51d01f97949e5c8cba2ddb2cf9b981bf9618e71 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 29 Sep 2014 18:37:14 +1000 Subject: [PATCH 110/254] AP_AHRS : add method to report if EKF is waiting to start --- libraries/AP_AHRS/AP_AHRS.h | 3 +++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 +++++ libraries/AP_AHRS/AP_AHRS_DCM.h | 3 +++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 5 +++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 5 files changed, 19 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index a1f96a482b8..e3c90f658ce 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -338,6 +338,9 @@ class AP_AHRS // is the AHRS subsystem healthy? virtual bool healthy(void) = 0; + // is the EKF waiting to start? + virtual bool ekfNotStarted(void) = 0; + protected: AHRS_VehicleClass _vehicle_class; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 9f79bcd45c6..00dc769f77f 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -937,3 +937,8 @@ bool AP_AHRS_DCM::healthy(void) // consider ourselves healthy if there have been no failures for 5 seconds return (_last_failure_ms == 0 || hal.scheduler->millis() - _last_failure_ms > 5000); } + +bool AP_AHRS_DCM::ekfNotStarted(void) +{ + return false; +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 76160c09e54..e1056e9ecc0 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -106,6 +106,9 @@ class AP_AHRS_DCM : public AP_AHRS // is the AHRS subsystem healthy? bool healthy(void); + // is the EKF waiting to start? + bool ekfNotStarted(void); + private: float _ki; float _ki_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 71ff44b17a4..1e749ee84c6 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -266,5 +266,10 @@ bool AP_AHRS_NavEKF::healthy(void) return AP_AHRS_DCM::healthy(); } +bool AP_AHRS_NavEKF::ekfNotStarted(void) +{ + return !ekf_started; +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 563535bcae5..e3246abee17 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -95,6 +95,9 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM // is the AHRS subsystem healthy? bool healthy(void); + // is the EKF waiting to start? + bool ekfNotStarted(void); + private: bool using_EKF(void) const; From 29c704fecc6927fbc43e983a48ce7e0f7d20c738 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Oct 2014 13:43:46 +0900 Subject: [PATCH 111/254] AHRS: rename ekfNotStarted method to initialised Also created default implementation in AP_AHRS class so AP_AHRS_DCM does not need to implement it. --- libraries/AP_AHRS/AP_AHRS.h | 4 ++-- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 ----- libraries/AP_AHRS/AP_AHRS_DCM.h | 3 --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 +++++--- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 5 +++-- 5 files changed, 10 insertions(+), 15 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e3c90f658ce..5689781b89c 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -338,8 +338,8 @@ class AP_AHRS // is the AHRS subsystem healthy? virtual bool healthy(void) = 0; - // is the EKF waiting to start? - virtual bool ekfNotStarted(void) = 0; + // true if the AHRS has completed initialisation + virtual bool initialised(void) const { return true; }; protected: AHRS_VehicleClass _vehicle_class; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 00dc769f77f..9f79bcd45c6 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -937,8 +937,3 @@ bool AP_AHRS_DCM::healthy(void) // consider ourselves healthy if there have been no failures for 5 seconds return (_last_failure_ms == 0 || hal.scheduler->millis() - _last_failure_ms > 5000); } - -bool AP_AHRS_DCM::ekfNotStarted(void) -{ - return false; -} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e1056e9ecc0..76160c09e54 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -106,9 +106,6 @@ class AP_AHRS_DCM : public AP_AHRS // is the AHRS subsystem healthy? bool healthy(void); - // is the EKF waiting to start? - bool ekfNotStarted(void); - private: float _ki; float _ki_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 1e749ee84c6..2930194bd3c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -266,10 +266,12 @@ bool AP_AHRS_NavEKF::healthy(void) return AP_AHRS_DCM::healthy(); } -bool AP_AHRS_NavEKF::ekfNotStarted(void) +// true if the AHRS has completed initialisation +bool AP_AHRS_NavEKF::initialised(void) const { - return !ekf_started; -} + // initialisation complete 10sec after ekf has started + return (ekf_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); +}; #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index e3246abee17..d03ffdc1732 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -28,6 +28,7 @@ #include #define AP_AHRS_NAVEKF_AVAILABLE 1 +#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started class AP_AHRS_NavEKF : public AP_AHRS_DCM { @@ -95,8 +96,8 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM // is the AHRS subsystem healthy? bool healthy(void); - // is the EKF waiting to start? - bool ekfNotStarted(void); + // true if the AHRS has completed initialisation + bool initialised(void) const; private: bool using_EKF(void) const; From 3b0a308ed2fa2f944eca1a570f440b64893b8aab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Oct 2014 13:44:02 +0900 Subject: [PATCH 112/254] Copter: only report ahrs unhealthy after initialisation --- ArduCopter/GCS_Mavlink.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 6dc344785d6..01ac407a573 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -206,7 +206,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; } - if (!ahrs.healthy()) { + if (ahrs.initialised() && !ahrs.healthy()) { // AHRS subsystem is unhealthy control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } From 400dd94ec57dafda3c5fc49a37ee8b965b7bb181 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Sep 2014 12:49:45 +0900 Subject: [PATCH 113/254] TradHeli: remove overall throttle level from landing check --- ArduCopter/control_land.pde | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 1092df79f90..cca30718e4f 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -204,7 +204,10 @@ static void update_land_detector() { // detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && motors.limit.throttle_lower && - (motors.get_throttle_out() < get_non_takeoff_throttle()) && (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) { +#if FRAME_CONFIG != HELI_FRAME + (motors.get_throttle_out() < get_non_takeoff_throttle()) && +#endif + (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) { if (!ap.land_complete) { // increase counter until we hit the trigger then set land complete flag if( land_detector < LAND_DETECTOR_TRIGGER) { From 6cbb9d635abf03324b00e7c255dcde74712f60af Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 3 Oct 2014 05:51:24 +1000 Subject: [PATCH 114/254] AP_NavEKF : Fix bug in reset of GPS glitch offset The GPS glitch offset was being zeroed during position resets. This caused the filter to reject subsequent GPS measurements if the GPS error persisted long enough to invoke a timeout and a position reset. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 42743169268..26b303b1cdd 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -411,8 +411,6 @@ void NavEKF::ResetPosition(void) state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay); state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay); } - // reset the glitch ofset correction states - gpsPosGlitchOffsetNE.zero(); // stored horizontal position states to prevent subsequent GPS measurements from being rejected for (uint8_t i=0; i<=49; i++){ storedStates[i].position[0] = state.position[0]; From e8368325953a1a7222cb887f1976d70ae9b86bfa Mon Sep 17 00:00:00 2001 From: lthall Date: Mon, 22 Sep 2014 21:32:37 +0900 Subject: [PATCH 115/254] Copter: increase autotune limits Rate D max to 0.020 (was 0.015) Rate P max to 0.35 (was 0.25) Stab P max to 20 (was 15) --- ArduCopter/control_autotune.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 8e53e1bac5d..0fb7cf96b20 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -53,10 +53,10 @@ #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing #define AUTOTUNE_RP_RATIO_FINAL 1.0f // I is set 1x P after testing #define AUTOTUNE_RD_MIN 0.002f // minimum Rate D value -#define AUTOTUNE_RD_MAX 0.015f // maximum Rate D value +#define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value #define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value -#define AUTOTUNE_RP_MAX 0.25f // maximum Rate P value -#define AUTOTUNE_SP_MAX 15.0f // maximum Stab P value +#define AUTOTUNE_RP_MAX 0.35f // maximum Rate P value +#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value #define AUTOTUNE_SUCCESS_COUNT 4 // how many successful iterations we need to freeze at current gains // Auto Tune message ids for ground station From 55173cc340e720ceb00a17df2745cfb75daba19b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 Oct 2014 23:27:11 +0900 Subject: [PATCH 116/254] Tri: _min_throttle interpreted as 0~1000 range for throttle_lower flag limit.throttle_lower flag becomes true when the throttle passed into the motors lib (which is in the 0 ~ 1000 range) is below _min throttle. This makes the interpretation of the THR_MIN parameter consistent between the main code (which uses 0 ~ 1000 range) and the motors lib (which previously used the RC3_MIN ~ RC3_MAX range). The remaining problem however is that the output of the motors continues to use THR_MIN as if it were a pwm. I don't believe this is a dangerous problem however. --- libraries/AP_Motors/AP_MotorsTri.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 0528a7f5bc9..da56f6c2c95 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -99,7 +99,14 @@ void AP_MotorsTri::output_armed() limit.throttle_lower = false; // Throttle is 0 to 1000 only - _rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle); + if (_rc_throttle.servo_out <= 0) { + _rc_throttle.servo_out = 0; + limit.throttle_lower = true; + } + if (_rc_throttle.servo_out >= _max_throttle) { + _rc_throttle.servo_out = _max_throttle; + limit.throttle_upper = true; + } // capture desired roll, pitch, yaw and throttle from receiver _rc_roll.calc_pwm(); @@ -120,15 +127,12 @@ void AP_MotorsTri::output_armed() motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_min + _spin_when_armed_ramped; motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed_ramped; - // Every thing is limited - limit.throttle_lower = true; - }else{ int16_t roll_out = (float)_rc_roll.pwm_out * 0.866f; int16_t pitch_out = _rc_pitch.pwm_out / 2; // check if throttle is below limit - if (_rc_throttle.radio_out <= out_min) { + if (_rc_throttle.servo_out <= _min_throttle) { limit.throttle_lower = true; } //left front From 51de79c2f84c06fdf013f5ab4c3933719bffd95e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 Oct 2014 23:28:32 +0900 Subject: [PATCH 117/254] MotorsMatrix: _min_throttle interpreted as 0 ~ 1000 range for throttle_lower flag Also trigger throttle_upper flag when throttle in reaches 1000 --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 903d7e14a66..79288283c2f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -145,11 +145,11 @@ void AP_MotorsMatrix::output_armed() // Throttle is 0 to 1000 only // To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object - if (_rc_throttle.servo_out < 0) { + if (_rc_throttle.servo_out <= 0) { _rc_throttle.servo_out = 0; limit.throttle_lower = true; } - if (_rc_throttle.servo_out > _max_throttle) { + if (_rc_throttle.servo_out >= _max_throttle) { _rc_throttle.servo_out = _max_throttle; limit.throttle_upper = true; } @@ -179,12 +179,11 @@ void AP_MotorsMatrix::output_armed() // Every thing is limited limit.roll_pitch = true; limit.yaw = true; - limit.throttle_lower = true; } else { // check if throttle is below limit - if (_rc_throttle.radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is + if (_rc_throttle.servo_out <= _min_throttle) { // perhaps being at min throttle itself is not a problem, only being under is limit.throttle_lower = true; } From 95b2b45a7bb38c686a287a7301855fba3fb0956d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 6 Oct 2014 11:48:00 +0900 Subject: [PATCH 118/254] Copter: ReleaseNotes for AC3.2-rc11 --- ArduCopter/ReleaseNotes.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index fad3b832870..937310bf502 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,16 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc11 06-Oct-2014 +Changes from 3.2-rc10 +1) reduce lean on take-off in Auto by resetting horizontal position targets +2) TradHeli landing check ignores overall throttle output +3) reduce AHRS bad messages by delaying 20sec after init to allow EKF to settle (Pixhawk only) +4) Bug fixes: + a) fix THR_MIN scaling issue that could cause landing-detector to fail to detect landing when ch3 min~max > 1000 pwm + b) fix Mediatek GPS configuration so update rate is set correctly to 5hz + c) fix to Condition-Yaw mission command to support relative angles + d) EKF bug fixes when recovering from GPS glitches (affects only Pixhawks using EKF) +------------------------------------------------------------------ ArduCopter 3.2-rc10 24-Sep-2014 Changes from 3.2-rc9 1) two-stage land-detector to reduce motor run-up when landing in Loiter, PosHold, RTL, Auto From 26b5321130b123c94fa1e1e64835dd19e138ffe8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 6 Oct 2014 11:48:13 +0900 Subject: [PATCH 119/254] Copter: version to AC3.2-rc11 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e7003686067..380af263968 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc10" +#define THISFIRMWARE "ArduCopter V3.2-rc11" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From a1e707b7f9e239de8ce8cca256865263b471a4fe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Sep 2014 14:41:26 +0900 Subject: [PATCH 120/254] Copter: cleanup enabling of cli and frsky telem for APM --- ArduCopter/APM_Config.h | 5 +---- ArduCopter/config.h | 2 ++ 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 870e1e9e13c..bb3f49ab08f 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -19,10 +19,6 @@ */ // uncomment the lines below to save on flash space if compiling for the APM using Arduino IDE. Top items save the most flash space -#if (CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_APM1) - # define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space - # define FRSKY_TELEM_ENABLED DISABLED // disable the FRSKY TELEM -#endif //#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space //#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash @@ -34,6 +30,7 @@ // features below are disabled by default on APM (but enabled on Pixhawk) //#define AC_RALLY ENABLED // disable rally points to save 2k of flash, and also frees rally point EEPROM for more mission commands //#define PARACHUTE ENABLED // enable parachute release at a cost of 1k of flash +//#define CLI_ENABLED ENABLED // enable the CLI (command-line-interface) at a cost of 21K of flash space // features below are disabled by default on all boards //#define OPTFLOW ENABLED // enable optical flow sensor and OF_LOITER flight mode at a cost of 5K of flash space diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9d6f2ca3481..84b52034668 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -84,6 +84,8 @@ #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 # define PARACHUTE DISABLED # define AC_RALLY DISABLED + # define CLI_ENABLED DISABLED + # define FRSKY_TELEM_ENABLED DISABLED #endif #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL From 5891cd3078d05c25a300e1dd06a0f81309753ad2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Oct 2014 14:13:35 +0900 Subject: [PATCH 121/254] Copter: disable sonar on APM1 and TradHeli on APM2 --- ArduCopter/config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 84b52034668..d06f63018e2 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -88,6 +88,11 @@ # define FRSKY_TELEM_ENABLED DISABLED #endif +// disable sonar on APM1 and TradHeli/APM2 +#if (CONFIG_HAL_BOARD == HAL_BOARD_APM1 || (CONFIG_HAL_BOARD == HAL_BOARD_APM2 && FRAME_CONFIG == HELI_FRAME)) + # define CONFIG_SONAR DISABLED +#endif + #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // low power CPUs (APM1, APM2 and SITL) # define MAIN_LOOP_RATE 100 From a95b8f1b0eacc3a5d161e2fee37d5d748b15fc8b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Oct 2014 21:06:43 +0900 Subject: [PATCH 122/254] Copter: Rate Pitch IMAX default to 1000 Spotted by Jonathan Challinger, thanks! --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d06f63018e2..2771c4c60e8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -620,7 +620,7 @@ # define RATE_PITCH_D 0.004f #endif #ifndef RATE_PITCH_IMAX - # define RATE_PITCH_IMAX 500 + # define RATE_PITCH_IMAX 1000 #endif #ifndef RATE_YAW_P From d309ecb587255ef91014136be7277533e32d8e5f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 20:17:31 +0900 Subject: [PATCH 123/254] INS: add gyro_calibrated_ok_all method This returns true if the gyros have been calibrated successfully --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 14 ++++++++++++++ libraries/AP_InertialSensor/AP_InertialSensor.h | 5 +++++ 2 files changed, 19 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 1f99d3079ad..43617189d46 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -306,6 +306,17 @@ bool AP_InertialSensor::get_gyro_health_all(void) const return (get_gyro_count() > 0); } +// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully +bool AP_InertialSensor::gyro_calibrated_ok_all() const +{ + for (uint8_t i=0; i 0); +} + // get_accel_health_all - return true if all accels are healthy bool AP_InertialSensor::get_accel_health_all(void) const { @@ -438,6 +449,7 @@ AP_InertialSensor::_init_gyro() best_diff[k] = 0; last_average[k].zero(); converged[k] = false; + _gyro_cal_ok[k] = true; // default calibration ok flag to true } for(int8_t c = 0; c < 5; c++) { @@ -513,6 +525,8 @@ AP_InertialSensor::_init_gyro() hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), (unsigned)k, ToDeg(best_diff[k])); _gyro_offset[k] = best_avg[k]; + // flag calibration as failed for this gyro + _gyro_cal_ok[k] = false; } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 70611c94d93..3eaba837557 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -120,6 +120,8 @@ class AP_InertialSensor bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } bool get_gyro_health_all(void) const; virtual uint8_t get_gyro_count(void) const { return 1; }; + bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; } + bool gyro_calibrated_ok_all() const; virtual bool get_accel_health(uint8_t instance) const { return true; } bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } @@ -224,6 +226,9 @@ class AP_InertialSensor // board orientation from AHRS enum Rotation _board_orientation; + + // calibrated_ok flags + bool _gyro_cal_ok[INS_MAX_INSTANCES]; }; #include "AP_InertialSensor_Oilpan.h" From eb594775b7c3a345ff46cbd9f7479206e49f0135 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 20:17:55 +0900 Subject: [PATCH 124/254] Copter: pre-arm check that gyro cal succeeded --- ArduCopter/motors.pde | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 342ae0362a2..b984ba54e62 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -378,6 +378,14 @@ static void pre_arm_checks(bool display_failure) return; } + // check gyros calibrated successfully + if(!ins.gyro_calibrated_ok_all()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyro cal failed")); + } + return; + } + #if INS_MAX_INSTANCES > 1 // check all gyros are consistent if (ins.get_gyro_count() > 1) { From 661755e05a066cc5fff09472ec3d98349741f8c2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 20:18:37 +0900 Subject: [PATCH 125/254] Copter: report gyro unhealthy if failed calibration --- ArduCopter/GCS_Mavlink.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 01ac407a573..8350ccf2de6 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -199,7 +199,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (ap.rc_receiver_present && !failsafe.radio) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } - if (!ins.get_gyro_health_all()) { + if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; } if (!ins.get_accel_health_all()) { From 7f8a68d44a07adff04e2569c1947f475fbf5f7ed Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 20:19:18 +0900 Subject: [PATCH 126/254] Copter: support pre-flight calibration of gyro --- ArduCopter/GCS_Mavlink.pde | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8350ccf2de6..b8416bee4f9 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1104,10 +1104,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_CALIBRATION: - if (packet.param1 == 1 || - packet.param2 == 1) { - ins.init_accel(); - ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim + if (packet.param1 == 1) { + // gyro offset calibration + ins.init_gyro(); result = MAV_RESULT_ACCEPTED; } if (packet.param3 == 1) { From 811e8571f187df998e2c8e5151dfe5b6f33d97f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 9 Oct 2014 18:30:20 +1100 Subject: [PATCH 127/254] AP_InertialNav: fixed use of _ahrs.get_relative_position_NED() with EKF disabled this prevents a floating point error caused by using an uninitialised vector3 when switching between DCM and EKF control in AP_InertialNav Pair-Programmed-With: Randy Mackay --- libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp | 2 +- libraries/AP_InertialNav/AP_InertialNav_NavEKF.h | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index d793cf95cb1..07e6c3390ec 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -25,7 +25,7 @@ void AP_InertialNav_NavEKF::init() void AP_InertialNav_NavEKF::update(float dt) { AP_InertialNav::update(dt); - _ahrs.get_relative_position_NED(_relpos_cm); + _ahrs_ekf.get_NavEKF().getPosNED(_relpos_cm); _relpos_cm *= 100; // convert to cm _haveabspos = _ahrs.get_position(_abspos); diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index bf954db509c..4f2170434b2 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -14,9 +14,10 @@ class AP_InertialNav_NavEKF : public AP_InertialNav { public: // Constructor - AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) : + AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) : AP_InertialNav(ahrs, baro, gps_glitch, baro_glitch), - _haveabspos(false) + _haveabspos(false), + _ahrs_ekf(ahrs) { } @@ -113,6 +114,7 @@ class AP_InertialNav_NavEKF : public AP_InertialNav Vector3f _velocity_cm; // NEU struct Location _abspos; bool _haveabspos; + AP_AHRS_NavEKF &_ahrs_ekf; }; #endif // __AP_INERTIALNAV_NAVEKF_H__ From 31c256bdd8e8236613a817f627dd573aafe75f59 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Oct 2014 16:43:24 +0900 Subject: [PATCH 128/254] AP_InertialNav: fixed use of ahrs.get_velocity with EKF disabled --- libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index 07e6c3390ec..585c82f873f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -30,7 +30,7 @@ void AP_InertialNav_NavEKF::update(float dt) _haveabspos = _ahrs.get_position(_abspos); - _ahrs.get_velocity_NED(_velocity_cm); + _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm); _velocity_cm *= 100; // convert to cm/s // InertialNav is NEU From 01a4ad24af0e07c9e9c26beabf852e1140aa37d1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Oct 2014 20:17:55 +0900 Subject: [PATCH 129/254] Copter: AC3.2-rc12 release notes --- ArduCopter/ReleaseNotes.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 937310bf502..b0a32f1b0cb 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,11 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc12 10-Oct-2014 +Changes from 3.2-rc11 +1) disable sonar on APM1 and TradHeli (APM1 & APM2) to allow code to fit +2) Add pre-arm and health check that gyro calibration succeeded +3) Bug fix to EKF reporting invalid position and velocity when switched on in flight with Ch7/Ch8 switch +------------------------------------------------------------------ ArduCopter 3.2-rc11 06-Oct-2014 Changes from 3.2-rc10 1) reduce lean on take-off in Auto by resetting horizontal position targets From 74e86a3cd79ed957725dd5f26f8cd4ea7495ebcb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Oct 2014 20:18:06 +0900 Subject: [PATCH 130/254] Copter: version to AC3.2-rc12 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 380af263968..3a2534b1849 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc11" +#define THISFIRMWARE "ArduCopter V3.2-rc12" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 6537432b506f10617a11f883948e7656aaadc7f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Oct 2014 22:42:47 +0900 Subject: [PATCH 131/254] Copter: auto-trim start delays auto-disarm by 15sec Fixes issue in which user only had 5 seconds after starting auto-trim to raise the throttle before the auto-disarm would kick-in. --- ArduCopter/motors.pde | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index b984ba54e62..eaa64d43929 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -5,6 +5,8 @@ #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define AUTO_DISARMING_DELAY 15 // called at 1hz so 15 seconds +static uint8_t auto_disarming_counter; + // arm_motors_check - checks for pilot input to arm or disarm the copter // called at 10hz static void arm_motors_check() @@ -68,6 +70,8 @@ static void arm_motors_check() // arm the motors and configure for flight if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) { auto_trim_counter = 250; + // ensure auto-disarm doesn't trigger immediately + auto_disarming_counter = 0; } // full left @@ -94,8 +98,6 @@ static void arm_motors_check() // called at 1hz static void auto_disarm_check() { - static uint8_t auto_disarming_counter; - // exit immediately if we are already disarmed or throttle is not zero if (!motors.armed() || g.rc_3.control_in > 0) { auto_disarming_counter = 0; From 4f427c6215567884273171a0f7678a1374d673cc Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 04:56:01 -0700 Subject: [PATCH 132/254] AC_PosControl: Protect from divide-by-zero in get_stopping_point_xy --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 77cc51e784d..cb107498428 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -475,7 +475,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const float vel_total = pythagorous2(curr_vel.x, curr_vel.y); // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero - if (kP <= 0.0f || _accel_cms <= 0.0f) { + if (kP <= 0.0f || _accel_cms <= 0.0f || vel_total == 0.0f) { stopping_point.x = curr_pos.x; stopping_point.y = curr_pos.y; return; From 71722d2e490e8c3496d5285fcd8307a4a21a6504 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:04:40 -0700 Subject: [PATCH 133/254] Copter: remove DRIFT and SPORT from manual_flight_mode function --- ArduCopter/flight_mode.pde | 2 -- 1 file changed, 2 deletions(-) diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 07761919913..a0f4fee88f0 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -259,8 +259,6 @@ static bool manual_flight_mode(uint8_t mode) { switch(mode) { case ACRO: case STABILIZE: - case DRIFT: - case SPORT: return true; default: return false; From 0dcf501766ca720464e7178883a47b6c8a1d23ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 10:12:50 +1100 Subject: [PATCH 134/254] AP_AHRS: fixed calls to DCM in parent class use_compass() and reset() are common to AP_AHRS_DCM and AP_AHRS_NavEKF. As AP_AHRS_NavEKF is a child of AP_AHRS_DCM, when we call use_compass() from within AP_AHRS_DCM we actually end up calling AP_AHRS_NavEKF::use_compass(). This has the effect of disabling the compass in DCM when EKF is active and EKF has decided not to use the compass. That means that the DCM yaw (and in fact the whole attitude) can get badly off while EKF is enabled, making DCM an ineffective fallback if EKF fails. The fix is to call the specific class versions of use_compass() and reset() --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 9f79bcd45c6..e925aa85a75 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -143,7 +143,7 @@ AP_AHRS_DCM::check_matrix(void) { if (_dcm_matrix.is_nan()) { //Serial.printf("ERROR: DCM matrix NAN\n"); - reset(true); + AP_AHRS_DCM::reset(true); return; } // some DCM matrix values can lead to an out of range error in @@ -161,7 +161,7 @@ AP_AHRS_DCM::check_matrix(void) // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", // _dcm_matrix.c.x); - reset(true); + AP_AHRS_DCM::reset(true); } } } @@ -242,7 +242,7 @@ AP_AHRS_DCM::normalize(void) // Our solution is blowing up and we will force back // to last euler angles _last_failure_ms = hal.scheduler->millis(); - reset(true); + AP_AHRS_DCM::reset(true); } } @@ -362,7 +362,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) float yaw_error; float yaw_deltat; - if (use_compass()) { + if (AP_AHRS_DCM::use_compass()) { /* we are using compass for yaw */ @@ -684,7 +684,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS - if (use_compass()) { + if (AP_AHRS_DCM::use_compass()) { if (have_gps() && gps_gain == 1.0f) { error[besti].z *= sinf(fabsf(roll)); } else { From f61ae9e9e52216eb96e2b480bd119c294dbe83b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 11:15:33 +1100 Subject: [PATCH 135/254] AP_AHRS: restore DCM attitude before update() The DCM drift correction code uses the current attitude to calculate error values to update its gyro drift correction. If we were using EKF then without this patch the DCM code running as an alternative AHRS source would be using the EKF attitude for calculating the error value, leading to very bad gyro drift estimation --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 2930194bd3c..6f872f59c2c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -52,6 +52,16 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const void AP_AHRS_NavEKF::update(void) { + // we need to restore the old DCM attitude values as these are + // used internally in DCM to calculate error values for gyro drift + // correction + roll = _dcm_attitude.x; + pitch = _dcm_attitude.y; + yaw = _dcm_attitude.z; + roll_sensor = degrees(roll)*100; + pitch_sensor = degrees(pitch)*100; + yaw_sensor = degrees(yaw)*100; + AP_AHRS_DCM::update(); // keep DCM attitude available for get_secondary_attitude() From ed30c0938e622491451ff0751dd5a266a5635db2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 13:18:08 +1100 Subject: [PATCH 136/254] AP_AHRS: use a common function for updating the CD values this ensures the wrapping of yaw is consistent between the 3 use cases --- libraries/AP_AHRS/AP_AHRS.cpp | 12 ++++++++++++ libraries/AP_AHRS/AP_AHRS.h | 3 +++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 7 +------ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 11 +++-------- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 033d5db14c8..bcffeca8087 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -247,3 +247,15 @@ void AP_AHRS::update_trig(void) _sin_pitch = -temp.c.x; _sin_roll = temp.c.y / _cos_pitch; } + +/* + update the centi-degree values + */ +void AP_AHRS::update_cd_values(void) +{ + roll_sensor = degrees(roll) * 100; + pitch_sensor = degrees(pitch) * 100; + yaw_sensor = degrees(yaw) * 100; + if (yaw_sensor < 0) + yaw_sensor += 36000; +} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5689781b89c..5caacf65074 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -367,6 +367,9 @@ class AP_AHRS // should be called after _dcm_matrix is updated void update_trig(void); + // update roll_sensor, pitch_sensor and yaw_sensor + void update_cd_values(void); + // pointer to compass object, if available Compass * _compass; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index e925aa85a75..fd015cdd07d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -837,12 +837,7 @@ AP_AHRS_DCM::euler_angles(void) _body_dcm_matrix.rotateXYinv(_trim); _body_dcm_matrix.to_euler(&roll, &pitch, &yaw); - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; - yaw_sensor = degrees(yaw) * 100; - - if (yaw_sensor < 0) - yaw_sensor += 36000; + update_cd_values(); } /* reporting of DCM state for MAVLink */ diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 6f872f59c2c..224e7a9ba7c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -58,9 +58,7 @@ void AP_AHRS_NavEKF::update(void) roll = _dcm_attitude.x; pitch = _dcm_attitude.y; yaw = _dcm_attitude.z; - roll_sensor = degrees(roll)*100; - pitch_sensor = degrees(pitch)*100; - yaw_sensor = degrees(yaw)*100; + update_cd_values(); AP_AHRS_DCM::update(); @@ -88,11 +86,8 @@ void AP_AHRS_NavEKF::update(void) roll = eulers.x; pitch = eulers.y; yaw = eulers.z; - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; - yaw_sensor = degrees(yaw) * 100; - if (yaw_sensor < 0) - yaw_sensor += 36000; + + update_cd_values(); update_trig(); // keep _gyro_bias for get_gyro_drift() From 470fcc2077bc230491660e1554df5217a4cdae5e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 16 Oct 2014 16:18:06 +0900 Subject: [PATCH 137/254] Copter: add DCM_CHECK_THRESH parameter --- ArduCopter/Parameters.h | 5 ++++- ArduCopter/Parameters.pde | 9 ++++++++- ArduCopter/config.h | 5 ++++- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b1fe5e4df76..27eb58783e8 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -120,7 +120,9 @@ class Parameters { k_param_ekfcheck_thresh, k_param_terrain, k_param_acro_expo, - k_param_throttle_deadzone, // 57 + k_param_throttle_deadzone, + k_param_optflow, + k_param_dcmcheck_thresh, // 59 // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -392,6 +394,7 @@ class Parameters { AP_Int8 land_repositioning; AP_Float ekfcheck_thresh; + AP_Float dcmcheck_thresh; #if FRAME_CONFIG == HELI_FRAME // Heli diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index f894f0a0cd6..9256c94010c 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -456,12 +456,19 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(land_repositioning, "LAND_REPOSITION", LAND_REPOSITION_DEFAULT), // @Param: EKF_CHECK_THRESH - // @DisplayName: EKF and InertialNav check compass and velocity variance threshold + // @DisplayName: EKF check compass and velocity variance threshold // @Description: Allows setting the maximum acceptable compass and velocity variance (0 to disable check) // @Values: 0:Disabled, 0.6:Default, 1.0:Relaxed // @User: Advanced GSCALAR(ekfcheck_thresh, "EKF_CHECK_THRESH", EKFCHECK_THRESHOLD_DEFAULT), + // @Param: DCM_CHECK_THRESH + // @DisplayName: DCM yaw error threshold + // @Description: Allows setting the maximum acceptable yaw error as a sin of the yaw error (0 to disable check) + // @Values: 0:Disabled, 0.8:Default, 0.98:Relaxed + // @User: Advanced + GSCALAR(dcmcheck_thresh, "DCM_CHECK_THRESH", DCMCHECK_THRESHOLD_DEFAULT), + #if FRAME_CONFIG == HELI_FRAME // @Group: HS1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2771c4c60e8..41efe38ed11 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -327,10 +327,13 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// EKF Checker +// EKF & DCM Checker #ifndef EKFCHECK_THRESHOLD_DEFAULT # define EKFCHECK_THRESHOLD_DEFAULT 0.8f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad #endif +#ifndef DCMCHECK_THRESHOLD_DEFAULT + # define DCMCHECK_THRESHOLD_DEFAULT 0.8f // DCM checker's default yaw error threshold above which we will abandon horizontal position hold. The units are sin(angle) so 0.8 = about 60degrees of error +#endif ////////////////////////////////////////////////////////////////////////////// // MAGNETOMETER From 7e1c975c5488a1f431816b2e0349832f9d324e28 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 16 Oct 2014 16:18:37 +0900 Subject: [PATCH 138/254] Copter: add DCM check of yaw error Triggers an "ekf" failsafe if the DCM yaw error is > 60deg --- ArduCopter/ArduCopter.pde | 4 +- ArduCopter/ekf_check.pde | 86 +++++++++++++++++++++++++++------------ 2 files changed, 63 insertions(+), 27 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3a2534b1849..99009628cc7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -807,7 +807,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 8, 10 }, { one_hz_loop, 400, 42 }, - { ekf_check, 40, 2 }, + { ekf_dcm_check, 40, 2 }, { crash_check, 40, 2 }, { gcs_check_input, 8, 550 }, { gcs_send_heartbeat, 400, 150 }, @@ -875,7 +875,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 2, 100 }, { one_hz_loop, 100, 420 }, - { ekf_check, 10, 20 }, + { ekf_dcm_check, 10, 20 }, { crash_check, 10, 20 }, { gcs_check_input, 2, 550 }, { gcs_send_heartbeat, 100, 150 }, diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index 11c59ac4868..7e2de943b81 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -15,42 +15,48 @@ # define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds #endif +// Enumerator for types of check +enum EKFCheckType { + CHECK_NONE = 0, + CHECK_DCM = 1, + CHECK_EKF = 2 +}; + //////////////////////////////////////////////////////////////////////////////// // EKF_check strucutre //////////////////////////////////////////////////////////////////////////////// static struct { - uint8_t fail_count_compass; // number of iterations ekf's compass variance has been out of tolerances + uint8_t fail_count_compass; // number of iterations ekf or dcm have been out of tolerances - uint8_t bad_compass : 1; // true if compass variance is bad + uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX) uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS } ekf_check_state; -// ekf_check - detects ekf variances that are out of tolerance +// ekf_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe // should be called at 10hz -void ekf_check() +void ekf_dcm_check() { -#if AP_AHRS_NAVEKF_AVAILABLE + EKFCheckType check_type = CHECK_NONE; + + // decide if we should check ekf or dcm + if (ahrs.have_inertial_nav() && g.ekfcheck_thresh > 0.0f) { + check_type = CHECK_EKF; + } else if (g.dcmcheck_thresh > 0.0f) { + check_type = CHECK_DCM; + } + // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected - if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !ahrs.have_inertial_nav() || ap.usb_connected) { + if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE) { ekf_check_state.fail_count_compass = 0; - ekf_check_state.bad_compass = 0; + ekf_check_state.bad_compass = false; AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; failsafe_ekf_off_event(); // clear failsafe return; } - // use EKF to get variance - float posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - float compass_variance; - float vel_variance; - ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); - compass_variance = magVar.length(); - // compare compass and velocity variance vs threshold - if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) { + if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) { // if compass is not yet flagged as bad if (!ekf_check_state.bad_compass) { // increase counter @@ -64,7 +70,11 @@ void ekf_check() Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + if (check_type == CHECK_EKF) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + } else { + gcs_send_text_P(SEVERITY_HIGH,PSTR("DCM bad heading")); + } ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -90,19 +100,46 @@ void ekf_check() AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; // To-Do: add ekf variances to extended status +} + +// dcm_over_threshold - returns true if the dcm yaw error is over the tolerance +static bool dcm_over_threshold() +{ + // return true if yaw error is over the threshold + return (g.dcmcheck_thresh > 0.0f && ahrs.get_error_yaw() > g.dcmcheck_thresh); +} + +// ekf_over_threshold - returns true if the ekf's variance are over the tolerance +static bool ekf_over_threshold() +{ +#if AP_AHRS_NAVEKF_AVAILABLE + // return false immediately if disabled + if (g.ekfcheck_thresh <= 0.0f) { + return false; + } + + // use EKF to get variance + float posVar, hgtVar, tasVar; + Vector3f magVar; + Vector2f offset; + float compass_variance; + float vel_variance; + ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); + compass_variance = magVar.length(); + + // return true if compass and velocity variance over the threshold + return (compass_variance >= g.ekfcheck_thresh && vel_variance >= g.ekfcheck_thresh); #else - // if no EKF available then never trigger failure - ekf_check_state.bad_compass = 0; - failsafe.ekf = false; + return false; #endif } -#if AP_AHRS_NAVEKF_AVAILABLE + // failsafe_ekf_event - perform ekf failsafe static void failsafe_ekf_event() { - // return immediately if ekf failsafe already triggered or disabled - if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) { + // return immediately if ekf failsafe already triggered + if (failsafe.ekf) { return; } @@ -138,4 +175,3 @@ static void failsafe_ekf_off_event(void) failsafe.ekf = false; Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); } -#endif From c093160ea972e4c921122ff1b126b5aab7620fc9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 Oct 2014 11:42:20 +1100 Subject: [PATCH 139/254] Copter: support logging while disarmed --- ArduCopter/ArduCopter.pde | 31 +++++++++++++++++-------------- ArduCopter/GCS_Mavlink.pde | 14 ++++++++++++-- ArduCopter/Log.pde | 12 ++++++------ ArduCopter/Parameters.h | 5 +++-- ArduCopter/Parameters.pde | 5 +++-- ArduCopter/commands.pde | 2 +- ArduCopter/commands_logic.pde | 4 ++-- ArduCopter/defines.h | 2 ++ ArduCopter/sensors.pde | 2 +- ArduCopter/system.pde | 19 +++++++++++++++++++ 10 files changed, 66 insertions(+), 30 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 99009628cc7..d1ed7d1fed6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -206,6 +206,9 @@ static AP_Notify notify; // used to detect MAVLink acks from GCS to stop compassmot static uint8_t command_ack_counter; +// has a log download started? +static bool in_log_download; + //////////////////////////////////////////////////////////////////////////////// // prototypes //////////////////////////////////////////////////////////////////////////////// @@ -944,7 +947,7 @@ static void barometer_accumulate(void) static void perf_update(void) { - if (g.log_bitmask & MASK_LOG_PM) + if (should_log(MASK_LOG_PM)) Log_Write_Performance(); if (scheduler.debug()) { cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"), @@ -1091,7 +1094,7 @@ static void update_batt_compass(void) compass.set_throttle((float)g.rc_3.servo_out/1000.0f); compass.read(); // log compass information - if (g.log_bitmask & MASK_LOG_COMPASS) { + if (should_log(MASK_LOG_COMPASS)) { Log_Write_Compass(); } } @@ -1104,16 +1107,16 @@ static void update_batt_compass(void) // should be run at 10hz static void ten_hz_logging_loop() { - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) { + if (should_log(MASK_LOG_ATTITUDE_MED)) { Log_Write_Attitude(); } - if (g.log_bitmask & MASK_LOG_RCIN) { + if (should_log(MASK_LOG_RCIN)) { DataFlash.Log_Write_RCIN(); } - if (g.log_bitmask & MASK_LOG_RCOUT) { + if (should_log(MASK_LOG_RCOUT)) { DataFlash.Log_Write_RCOUT(); } - if ((g.log_bitmask & MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { + if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { Log_Write_Nav_Tuning(); } } @@ -1128,11 +1131,11 @@ static void fifty_hz_logging_loop() #endif #if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) { + if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } - if (g.log_bitmask & MASK_LOG_IMU) { + if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(ins); } #endif @@ -1162,12 +1165,12 @@ static void three_hz_loop() // one_hz_loop - runs at 1Hz static void one_hz_loop() { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { Log_Write_Data(DATA_AP_STATE, ap.value); } // log battery info to the dataflash - if (g.log_bitmask & MASK_LOG_CURRENT) { + if (should_log(MASK_LOG_CURRENT)) { Log_Write_Current(); } @@ -1226,7 +1229,7 @@ static void update_optical_flow(void) of_log_counter++; if( of_log_counter >= 4 ) { of_log_counter = 0; - if (g.log_bitmask & MASK_LOG_OPTFLOW) { + if (should_log(MASK_LOG_OPTFLOW)) { Log_Write_Optflow(); } } @@ -1250,7 +1253,7 @@ static void update_GPS(void) last_gps_reading[i] = gps.last_message_time_ms(i); // log GPS message - if (g.log_bitmask & MASK_LOG_GPS) { + if (should_log(MASK_LOG_GPS)) { DataFlash.Log_Write_GPS(gps, i, current_loc.alt); } @@ -1336,7 +1339,7 @@ init_simple_bearing() super_simple_sin_yaw = simple_sin_yaw; // log the simple bearing to dataflash - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { Log_Write_Data(DATA_INIT_SIMPLE_BEARING, ahrs.yaw_sensor); } } @@ -1407,7 +1410,7 @@ static void update_altitude() sonar_alt = read_sonar(); // write altitude info to dataflash logs - if (g.log_bitmask & MASK_LOG_CTUN) { + if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); } } diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b8416bee4f9..5d0da2f75c1 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1291,11 +1291,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 { - handle_radio_status(msg, DataFlash, (g.log_bitmask & MASK_LOG_PM) != 0); + handle_radio_status(msg, DataFlash, should_log(MASK_LOG_PM)); break; } - case MAVLINK_MSG_ID_LOG_REQUEST_LIST ... MAVLINK_MSG_ID_LOG_REQUEST_END: // MAV ID: 117 ... 122 + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + case MAVLINK_MSG_ID_LOG_ERASE: + in_log_download = true; + // fallthru + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + if (!in_mavlink_delay && !motors.armed()) { + handle_log_message(msg, DataFlash); + } + break; + case MAVLINK_MSG_ID_LOG_REQUEST_END: + in_log_download = false; if (!in_mavlink_delay && !motors.armed()) { handle_log_message(msg, DataFlash); } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 0f0c8a0fb5d..d83a9b90c0d 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -529,7 +529,7 @@ struct PACKED log_Event { // Wrote an event packet static void Log_Write_Event(uint8_t id) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Event pkt = { LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG), id : id @@ -547,7 +547,7 @@ struct PACKED log_Data_Int16t { // Write an int16_t data packet static void Log_Write_Data(uint8_t id, int16_t value) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Data_Int16t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG), id : id, @@ -566,7 +566,7 @@ struct PACKED log_Data_UInt16t { // Write an uint16_t data packet static void Log_Write_Data(uint8_t id, uint16_t value) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt16t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG), id : id, @@ -585,7 +585,7 @@ struct PACKED log_Data_Int32t { // Write an int32_t data packet static void Log_Write_Data(uint8_t id, int32_t value) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Data_Int32t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG), id : id, @@ -604,7 +604,7 @@ struct PACKED log_Data_UInt32t { // Write a uint32_t data packet static void Log_Write_Data(uint8_t id, uint32_t value) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt32t pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG), id : id, @@ -623,7 +623,7 @@ struct PACKED log_Data_Float { // Write a float data packet static void Log_Write_Data(uint8_t id, float value) { - if (g.log_bitmask != 0) { + if (should_log(MASK_LOG_ANY)) { struct log_Data_Float pkt = { LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG), id : id, diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 27eb58783e8..33de5d229ab 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -81,7 +81,7 @@ class Parameters { // Misc // - k_param_log_bitmask = 20, + k_param_log_bitmask_old = 20, // Deprecated k_param_log_last_filenumber, // *** Deprecated - remove // with next eeprom number // change @@ -123,6 +123,7 @@ class Parameters { k_param_throttle_deadzone, k_param_optflow, k_param_dcmcheck_thresh, // 59 + k_param_log_bitmask, // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -382,7 +383,7 @@ class Parameters { // Misc // - AP_Int16 log_bitmask; + AP_Int32 log_bitmask; AP_Int8 esc_calibrate; AP_Int8 radio_tuning; AP_Int16 radio_tuning_high; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 9256c94010c..d2fbba378fc 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -354,8 +354,8 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask - // @Description: 2 byte bitmap of log types to enable - // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll,0:Disabled + // @Description: 4 byte bitmap of log types to enable + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,59390:NearlyAll,131071:All+DisarmedLogging,0:Disabled // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -1142,6 +1142,7 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = { { Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" }, { Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" }, { Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" }, + { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, }; static void load_parameters(void) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 7a475f01c80..2481a0b7d01 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -14,7 +14,7 @@ static void init_home() inertial_nav.setup_home_position(); // log new home position which mission library will pull from ahrs - if (g.log_bitmask & MASK_LOG_CMD) { + if (should_log(MASK_LOG_CMD)) { AP_Mission::Mission_Command temp_cmd; if (mission.read_cmd_from_storage(0, temp_cmd)) { Log_Write_Cmd(temp_cmd); diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 3d5968c097b..fafd0959883 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -33,7 +33,7 @@ static void auto_spline_start(const Vector3f& destination, bool stopped_at_start static bool start_command(const AP_Mission::Mission_Command& cmd) { // To-Do: logging when new commands start/end - if (g.log_bitmask & MASK_LOG_CMD) { + if (should_log(MASK_LOG_CMD)) { Log_Write_Cmd(cmd); } @@ -914,7 +914,7 @@ static void do_take_picture() { #if CAMERA == ENABLED camera.trigger_pic(); - if (g.log_bitmask & MASK_LOG_CAMERA) { + if (should_log(MASK_LOG_CAMERA)) { DataFlash.Log_Write_Camera(ahrs, gps, current_loc); } #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index e6f4f197aa8..5e9cdbbe5d4 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -263,6 +263,8 @@ enum FlipState { #define MASK_LOG_COMPASS (1<<13) #define MASK_LOG_INAV (1<<14) // deprecated #define MASK_LOG_CAMERA (1<<15) +#define MASK_LOG_WHEN_DISARMED (1UL<<16) +#define MASK_LOG_ANY 0xFFFF // DATA - event logging #define DATA_MAVLINK_FLOAT 1 diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 03aea807820..880197f2892 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -22,7 +22,7 @@ static void init_barometer(bool full_calibration) static int32_t read_barometer(void) { barometer.read(); - if (g.log_bitmask & MASK_LOG_IMU) { + if (should_log(MASK_LOG_IMU)) { Log_Write_Baro(); } int32_t balt = barometer.get_altitude() * 100.0f; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 0f2cba66f2a..919f527e0d3 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -404,3 +404,22 @@ static void telemetry_send(void) (AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get()); #endif } + +/* + should we log a message type now? + */ +static bool should_log(uint32_t mask) +{ + if (!(mask & g.log_bitmask) || in_mavlink_delay) { + return false; + } + bool ret = motors.armed() || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; + if (ret && !DataFlash.logging_started() && !in_log_download) { + // we have to set in_mavlink_delay to prevent logging while + // writing headers + in_mavlink_delay = true; + start_logging(); + in_mavlink_delay = false; + } + return ret; +} From d58f7ada623f7dd4f417735db816707ee356952c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 17 Oct 2014 16:07:47 +0900 Subject: [PATCH 140/254] Copter: remove extra in_mavlink_delay from should_log function Also return false when logging disabled --- ArduCopter/system.pde | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 919f527e0d3..4ab9ff34872 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -410,6 +410,7 @@ static void telemetry_send(void) */ static bool should_log(uint32_t mask) { +#if LOGGING_ENABLED == ENABLED if (!(mask & g.log_bitmask) || in_mavlink_delay) { return false; } @@ -417,9 +418,10 @@ static bool should_log(uint32_t mask) if (ret && !DataFlash.logging_started() && !in_log_download) { // we have to set in_mavlink_delay to prevent logging while // writing headers - in_mavlink_delay = true; start_logging(); - in_mavlink_delay = false; } return ret; +#else + return false; +#endif } From 92225dc5dbb2f8831425a2f6f90233bb9ff9b65c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 17 Oct 2014 15:46:12 +0900 Subject: [PATCH 141/254] Copter: add NearlyAll-AC315 LOG_BITMASK description --- ArduCopter/Parameters.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index d2fbba378fc..51c957cbaa5 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -355,7 +355,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: 4 byte bitmap of log types to enable - // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,59390:NearlyAll,131071:All+DisarmedLogging,0:Disabled + // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,43006:NearlyAll,131070:All+DisarmedLogging,0:Disabled // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), From 820b4e2bedb1c2765ec137b480fd5c4b787402e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 09:16:31 +1100 Subject: [PATCH 142/254] AP_Compass: added set_offsets() interface this will be used by Replay to prevent the need for saving parameters --- libraries/AP_Compass/Compass.cpp | 9 +++++++++ libraries/AP_Compass/Compass.h | 7 +++++++ 2 files changed, 16 insertions(+) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 0df6cc0f3c9..33b9ac304c8 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -169,6 +169,15 @@ Compass::init() return true; } +void +Compass::set_offsets(uint8_t i, const Vector3f &offsets) +{ + // sanity check compass instance provided + if (i < COMPASS_MAX_INSTANCES) { + _offset[i].set(offsets); + } +} + void Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) { diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 71b3fdb4b53..1a0af229278 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -87,6 +87,13 @@ class Compass /// float calculate_heading(const Matrix3f &dcm_matrix) const; + /// Sets offset x/y/z values. + /// + /// @param i compass instance + /// @param offsets Offsets to the raw mag_ values. + /// + void set_offsets(uint8_t i, const Vector3f &offsets); + /// Sets and saves the compass offset x/y/z values. /// /// @param i compass instance From bbb6471277c63f4bc0de546320bd8e4ce1755841 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 09:17:22 +1100 Subject: [PATCH 143/254] Replay: fixed loading of users parameters and parameter override use compass.set_offsets() to avoid trying to write to storage --- Tools/Replay/LogReader.cpp | 22 +++++++++++++++++++--- Tools/Replay/LogReader.h | 3 +++ Tools/Replay/Parameters.h | 3 ++- Tools/Replay/Parameters.pde | 4 ++++ Tools/Replay/Replay.pde | 9 +++++++-- 5 files changed, 35 insertions(+), 6 deletions(-) diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 1da4a02cac8..1eadb4cc292 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -144,7 +144,7 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -186,7 +186,7 @@ void LogReader::process_rover(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -216,7 +216,7 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -250,6 +250,10 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length) bool LogReader::set_parameter(const char *name, float value) { + if (strcmp(name, "GPS_TYPE") == 0) { + // ignore this one + return true; + } enum ap_var_type var_type; AP_Param *vp = AP_Param::find(name, &var_type); if (vp == NULL) { @@ -470,6 +474,18 @@ bool LogReader::update(uint8_t &type) break; } + case LOG_AHR2_MSG: { + struct log_AHRS msg; + if(sizeof(msg) != f.length) { + printf("Bad AHR2 length %u should be %u\n", (unsigned)f.length, (unsigned)sizeof(msg)); + exit(1); + } + memcpy(&msg, data, sizeof(msg)); + wait_timestamp(msg.time_ms); + ahr2_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f); + break; + } + default: if (vehicle == VEHICLE_PLANE) { diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 9fc9898fab8..15e23911917 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -3,6 +3,7 @@ enum log_messages { // plane specific messages LOG_PLANE_ATTITUDE_MSG = 9, LOG_PLANE_COMPASS_MSG = 11, + LOG_PLANE_COMPASS2_MSG = 15, LOG_PLANE_AIRSPEED_MSG = 17, // copter specific messages @@ -25,6 +26,7 @@ class LogReader bool wait_type(uint8_t type); const Vector3f &get_attitude(void) const { return attitude; } + const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; } const Vector3f &get_inavpos(void) const { return inavpos; } const Vector3f &get_sim_attitude(void) const { return sim_attitude; } const float &get_relalt(void) const { return rel_altitude; } @@ -57,6 +59,7 @@ class LogReader struct log_Format formats[LOGREADER_MAX_FORMATS]; Vector3f attitude; + Vector3f ahr2_attitude; Vector3f sim_attitude; Vector3f inavpos; float rel_altitude; diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h index 3a79b0f17bf..e445c2defb9 100644 --- a/Tools/Replay/Parameters.h +++ b/Tools/Replay/Parameters.h @@ -15,7 +15,8 @@ class Parameters { k_param_ins, k_param_ahrs, k_param_airspeed, - k_param_NavEKF + k_param_NavEKF, + k_param_compass }; AP_Int8 dummy; }; diff --git a/Tools/Replay/Parameters.pde b/Tools/Replay/Parameters.pde index 23561cabed2..94b2672a7ac 100644 --- a/Tools/Replay/Parameters.pde +++ b/Tools/Replay/Parameters.pde @@ -34,6 +34,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF), + // @Group: COMPASS_ + // @Path: ../libraries/AP_Compass/AP_Compass.cpp + GOBJECT(compass, "COMPASS_", Compass), + AP_VAREND }; diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index c37628e42e2..d4df21fbf17 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -103,6 +103,8 @@ static struct { float value; } user_parameters[100]; +// setup the var_info table +AP_Param param_loader(var_info); static void usage(void) { @@ -228,7 +230,7 @@ void setup() ekf3f = fopen("EKF3.dat", "w"); ekf4f = fopen("EKF4.dat", "w"); - fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); + fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n"); fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); @@ -391,10 +393,13 @@ void loop() barometer.get_altitude(), LogReader.get_attitude().x, LogReader.get_attitude().y, - LogReader.get_attitude().z, + wrap_180_cd(LogReader.get_attitude().z*100)*0.01f, LogReader.get_inavpos().x, LogReader.get_inavpos().y, LogReader.get_relalt(), + LogReader.get_ahr2_attitude().x, + LogReader.get_ahr2_attitude().y, + wrap_180_cd(LogReader.get_ahr2_attitude().z*100)*0.01f, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z), From cd35293a7b4e2faf82f80377455f17944eed65b5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Oct 2014 17:12:50 +0900 Subject: [PATCH 144/254] Parachute: set servo or relay to off position on every update This resolves the issue of the parachute servo's position not being initialised to the off position due to an ordering problem of the auxiliary servos being initialised after the parachute object. --- libraries/AP_Parachute/AP_Parachute.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 73dae0dc92b..9cfe8111bf8 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -83,7 +83,7 @@ void AP_Parachute::release() void AP_Parachute::update() { // exit immediately if not enabled or parachute not to be released - if (_enabled <= 0 || _release_time == 0) { + if (_enabled <= 0) { return; } @@ -91,7 +91,7 @@ void AP_Parachute::update() uint32_t time_diff = hal.scheduler->millis() - _release_time; // check if we should release parachute - if (!_released) { + if ((_release_time != 0) && !_released) { if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS) { if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) { // move servo @@ -102,7 +102,7 @@ void AP_Parachute::update() } _released = true; } - }else if (time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS + AP_PARACHUTE_RELEASE_DURATION_MS) { + }else if ((_release_time == 0) || time_diff >= AP_PARACHUTE_RELEASE_DELAY_MS + AP_PARACHUTE_RELEASE_DURATION_MS) { if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_SERVO) { // move servo back to off position RC_Channel_aux::set_radio(RC_Channel_aux::k_parachute_release, _servo_off_pwm); From 9d76d3b423c33ef4a01821d63160e7e111d68ba8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Oct 2014 20:08:46 +0900 Subject: [PATCH 145/254] Copter: log DCM reported roll-pitch and yaw error --- ArduCopter/Log.pde | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d83a9b90c0d..9569605f6c6 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -464,6 +464,8 @@ struct PACKED log_Attitude { int16_t pitch; uint16_t control_yaw; uint16_t yaw; + uint16_t error_rp; + uint16_t error_yaw; }; // Write an attitude packet @@ -478,7 +480,9 @@ static void Log_Write_Attitude() control_pitch : (int16_t)targets.y, pitch : (int16_t)ahrs.pitch_sensor, control_yaw : (uint16_t)targets.z, - yaw : (uint16_t)ahrs.yaw_sensor + yaw : (uint16_t)ahrs.yaw_sensor, + error_rp : (uint16_t)(ahrs.get_error_rp() * 100), + error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -684,7 +688,7 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" }, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), - "ATT", "IccccCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw" }, + "ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }, { LOG_MODE_MSG, sizeof(log_Mode), "MODE", "Mh", "Mode,ThrCrs" }, { LOG_STARTUP_MSG, sizeof(log_Startup), From d71b08af0c058888e30126f0f874aa80077d5c5e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Oct 2014 20:54:08 +0900 Subject: [PATCH 146/254] AP_Motors: reduce slow-start increment for fast CPUs --- libraries/AP_Motors/AP_Motors_Class.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index befa01a1d26..315925ac1c3 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -61,10 +61,16 @@ #define AP_MOTOR_THROTTLE_LIMIT 0x04 #define AP_MOTOR_ANY_LIMIT 0xFF -// slow start increments - throttle increase per (100hz) iteration. i.e. 5 = full speed in 2 seconds -#define AP_MOTOR_SLOW_START_INCREMENT 10 // max throttle ramp speed (i.e. motors can reach full throttle in 2 seconds) -#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 2 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second) - +// To-Do: replace this hard coded counter with a timer +#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX + // slow start increments - throttle increase per (100hz) iteration. i.e. 5 = full speed in 2 seconds + #define AP_MOTOR_SLOW_START_INCREMENT 10 // max throttle ramp speed (i.e. motors can reach full throttle in 1 second) + #define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 2 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second) +#else + // slow start increments - throttle increase per (400hz) iteration. i.e. 1 = full speed in 2.5 seconds + #define AP_MOTOR_SLOW_START_INCREMENT 3 // max throttle ramp speed (i.e. motors can reach full throttle in 0.8 seconds) + #define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 1 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second) +#endif /// @class AP_Motors class AP_Motors { public: From 8da15cb4098dc31552f5d410ccbd0ed42b1543b5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 27 Sep 2014 11:44:01 +0900 Subject: [PATCH 147/254] Copter: check target of set-mode request from GCS Issue discovered and fix contributed by Deadolous --- ArduCopter/GCS_Mavlink.pde | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 5d0da2f75c1..fb02a9b6277 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -884,6 +884,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); + // exit immediately if this command is not meant for this vehicle + if (mavlink_check_target(packet.target_system, 0)) { + break; + } + // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes if (packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { if (set_mode(packet.custom_mode)) { From d37c7883940c55a98db473fcfcfa191f541f4464 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 12 Oct 2014 22:17:38 +1100 Subject: [PATCH 148/254] AP_NavEKF: Track baro alt when pre-armed This will help prevent spurious alt disparity warning messages for copter --- libraries/AP_NavEKF/AP_NavEKF.cpp | 33 +++++++++++++++---------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 26b303b1cdd..5fde16ab419 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -699,23 +699,6 @@ void NavEKF::SelectVelPosFusion() fusePosData = false; } - // check for and read new height data - readHgtData(); - - // command fusion of height data - if (newDataHgt) - { - // reset data arrived flag - newDataHgt = false; - // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing - memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); - hgtUpdateCount = 0; - // enable fusion - fuseHgtData = true; - } else { - fuseHgtData = false; - } - } else { // in static mode use synthetic position measurements set to zero // only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration @@ -726,7 +709,23 @@ void NavEKF::SelectVelPosFusion() fusePosData = false; } fuseVelData = false; + } + + // check for and read new height data + readHgtData(); + + // command fusion of height data + if (newDataHgt) + { + // reset data arrived flag + newDataHgt = false; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); + hgtUpdateCount = 0; + // enable fusion fuseHgtData = true; + } else { + fuseHgtData = false; } // perform fusion From 3638bfb614e39f06f608c7eb6e6901dceb3f35d9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 21 Oct 2014 11:32:20 +0900 Subject: [PATCH 149/254] AC_PosControl: bug fix dt calculation fixes issue in which now could be earlier than _last_update_xy_ms leading to a large dt value and a sudden lean on takeoff --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index cb107498428..37714efaaca 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -551,6 +551,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity) uint32_t now = hal.scheduler->millis(); if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) { init_xy_controller(); + now = _last_update_xy_ms; } // check if xy leash needs to be recalculated From 65472eaf624d759c2e68ed271c428a411490e1cf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 21 Oct 2014 21:40:16 +0900 Subject: [PATCH 150/254] AHRS_DCM: sanity check AHRS_RP_P and AHRS_YAW_P --- libraries/AP_AHRS/AP_AHRS.h | 2 ++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++++++++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5caacf65074..f644c2de929 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -32,6 +32,8 @@ #include #define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees +#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter +#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter enum AHRS_VehicleClass { AHRS_VEHICLE_UNKNOWN, diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index fd015cdd07d..04d83ea6bc1 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -442,6 +442,11 @@ AP_AHRS_DCM::drift_correction_yaw(void) // integration at higher rates float spin_rate = _omega.length(); + // sanity check _kp_yaw + if (_kp_yaw < AP_AHRS_YAW_P_MIN) { + _kp_yaw = AP_AHRS_YAW_P_MIN; + } + // update the proportional control to drag the // yaw back to the right value. We use a gain // that depends on the spin rate. See the fastRotations.pdf @@ -715,6 +720,11 @@ AP_AHRS_DCM::drift_correction(float deltat) // base the P gain on the spin rate float spin_rate = _omega.length(); + // sanity check _kp value + if (_kp < AP_AHRS_RP_P_MIN) { + _kp = AP_AHRS_RP_P_MIN; + } + // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. From ba5e36817517d95ff60ecb5bd37aa2fa741b2f11 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 21 Oct 2014 22:09:42 +0900 Subject: [PATCH 151/254] AC_WPNav: bug fix sanity check of set_speed_xy This corrects a bug that allowed the waypoint speed to be set to zero --- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 3fd32c0d0d1..dee9690c66b 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -341,7 +341,7 @@ void AC_WPNav::wp_and_spline_init() void AC_WPNav::set_speed_xy(float speed_cms) { // range check new target speed and update position controller - if (_wp_speed_cms >= WPNAV_WP_SPEED_MIN) { + if (speed_cms >= WPNAV_WP_SPEED_MIN) { _wp_speed_cms = speed_cms; _pos_control.set_speed_xy(_wp_speed_cms); // flag that wp leash must be recalculated From 03351386836653d6d285b202a26f33ecc3dc3aad Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 26 Sep 2014 22:35:02 -0700 Subject: [PATCH 152/254] Copter: print frame type in log headers --- ArduCopter/GCS_Mavlink.pde | 1 + ArduCopter/Log.pde | 4 +++- ArduCopter/config.h | 22 ++++++++++++++++++++++ 3 files changed, 26 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index fb02a9b6277..c4a428176cd 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -915,6 +915,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text_P(SEVERITY_LOW, PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION)); #endif + send_text_P(SEVERITY_LOW, PSTR("Frame: " FRAME_CONFIG_STRING)); handle_param_request_list(msg); break; } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 9569605f6c6..3142665f042 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -718,7 +718,8 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) #endif cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING - "\nFree RAM: %u\n"), + "\nFree RAM: %u\n" + "\nFrame: " FRAME_CONFIG_STRING "\n"), (unsigned) hal.util->available_memory()); cliSerial->println_P(PSTR(HAL_BOARD_NAME)); @@ -749,6 +750,7 @@ static void start_logging() if (hal.util->get_system_id(sysid)) { DataFlash.Log_Write_Message(sysid); } + DataFlash.Log_Write_Message_P(PSTR("Frame: " FRAME_CONFIG_STRING)); // log the flight mode Log_Write_Mode(control_mode); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 41efe38ed11..8b14d36d050 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -117,6 +117,28 @@ # define FRAME_CONFIG QUAD_FRAME #endif +#if FRAME_CONFIG == QUAD_FRAME + # define FRAME_CONFIG_STRING "QUAD" +#elif FRAME_CONFIG == TRI_FRAME + # define FRAME_CONFIG_STRING "TRI" +#elif FRAME_CONFIG == HEXA_FRAME + # define FRAME_CONFIG_STRING "HEXA" +#elif FRAME_CONFIG == Y6_FRAME + # define FRAME_CONFIG_STRING "Y6" +#elif FRAME_CONFIG == OCTA_FRAME + # define FRAME_CONFIG_STRING "OCTA" +#elif FRAME_CONFIG == OCTA_QUAD_FRAME + # define FRAME_CONFIG_STRING "OCTA_QUAD" +#elif FRAME_CONFIG == HELI_FRAME + # define FRAME_CONFIG_STRING "HELI" +#elif FRAME_CONFIG == SINGLE_FRAME + # define FRAME_CONFIG_STRING "SINGLE" +#elif FRAME_CONFIG == COAX_FRAME + # define FRAME_CONFIG_STRING "COAX" +#else + # define FRAME_CONFIG_STRING "UNKNOWN" +#endif + ///////////////////////////////////////////////////////////////////////////////// // TradHeli defaults #if FRAME_CONFIG == HELI_FRAME From 0d2954b5a40b761c25101f9e929986985e88263f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 Oct 2014 16:07:10 +0900 Subject: [PATCH 153/254] Copter: landing detector checks baro climb rate Barometer climb rate must be -150cm/s ~ +150cm/s This threshold is generous because we already use the inertial navigation climb rate so this is just to catch cases where inertial nav is very incorrect in it's climbrate estimates --- ArduCopter/ArduCopter.pde | 6 +++--- ArduCopter/config.h | 3 +++ ArduCopter/control_land.pde | 4 +++- ArduCopter/sensors.pde | 8 +++----- ArduCopter/test.pde | 4 ++-- 5 files changed, 14 insertions(+), 11 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d1ed7d1fed6..2129fc2fc9d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -576,8 +576,8 @@ static int16_t climb_rate; static int16_t sonar_alt; static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar static float target_sonar_alt; // desired altitude in cm above the ground -// The altitude as reported by Baro in cm - Values can be quite high -static int32_t baro_alt; +static int32_t baro_alt; // barometer altitude in cm above home +static float baro_climbrate; // barometer climbrate in cm/s //////////////////////////////////////////////////////////////////////////////// @@ -1404,7 +1404,7 @@ static void read_AHRS(void) static void update_altitude() { // read in baro altitude - baro_alt = read_barometer(); + read_barometer(); // read in sonar altitude sonar_alt = read_sonar(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 8b14d36d050..5c2496ab3d6 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -500,6 +500,9 @@ #ifndef LAND_DETECTOR_CLIMBRATE_MAX # define LAND_DETECTOR_CLIMBRATE_MAX 30 // vehicle climb rate must be between -30 and +30 cm/s #endif +#ifndef LAND_DETECTOR_BARO_CLIMBRATE_MAX +# define LAND_DETECTOR_BARO_CLIMBRATE_MAX 150 // barometer climb rate must be between -150cm/s ~ +150cm/s +#endif #ifndef LAND_DETECTOR_ROTATION_MAX # define LAND_DETECTOR_ROTATION_MAX 0.50f // vehicle rotation must be below 0.5 rad/sec (=30deg/sec for) vehicle to consider itself landed #endif diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index cca30718e4f..ec37b3f0d61 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -203,7 +203,9 @@ static bool land_complete_maybe() static void update_land_detector() { // detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate - if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && motors.limit.throttle_lower && + if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && + (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX) && + motors.limit.throttle_lower && #if FRAME_CONFIG != HELI_FRAME (motors.get_throttle_out() < get_non_takeoff_throttle()) && #endif diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 880197f2892..90af17e0ea8 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -19,13 +19,14 @@ static void init_barometer(bool full_calibration) } // return barometric altitude in centimeters -static int32_t read_barometer(void) +static void read_barometer(void) { barometer.read(); if (should_log(MASK_LOG_IMU)) { Log_Write_Baro(); } - int32_t balt = barometer.get_altitude() * 100.0f; + baro_alt = barometer.get_altitude() * 100.0f; + baro_climbrate = barometer.get_climb_rate() * 100.0f; // run glitch protection and update AP_Notify if home has been initialised baro_glitch.check_alt(); @@ -38,9 +39,6 @@ static int32_t read_barometer(void) } AP_Notify::flags.baro_glitching = report_baro_glitch; } - - // return altitude - return balt; } // return sonar altitude in centimeters diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d59c10248aa..efc444e67f3 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -58,13 +58,13 @@ test_baro(uint8_t argc, const Menu::arg *argv) while(1) { delay(100); - alt = read_barometer(); + read_barometer(); if (!barometer.healthy()) { cliSerial->println_P(PSTR("not healthy")); } else { cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"), - alt / 100.0, + baro_alt / 100.0, barometer.get_pressure(), barometer.get_temperature()); } From 5cbcbf9b37bf4ceaf09ae48d9080abca8f2a6fb8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 Oct 2014 16:29:25 +0900 Subject: [PATCH 154/254] DataFlash: log baro climbrate --- libraries/DataFlash/DataFlash.h | 3 ++- libraries/DataFlash/LogFile.cpp | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 2864478ffcd..6f74c7c78f4 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -251,6 +251,7 @@ struct PACKED log_BARO { float altitude; float pressure; int16_t temperature; + float climbrate; }; struct PACKED log_AHRS { @@ -431,7 +432,7 @@ struct PACKED log_Ubx2 { { LOG_RCOUT_MSG, sizeof(log_RCOUT), \ "RCOU", "Ihhhhhhhh", "TimeMS,Chan1,Chan2,Chan3,Chan4,Chan5,Chan6,Chan7,Chan8" }, \ { LOG_BARO_MSG, sizeof(log_BARO), \ - "BARO", "Iffc", "TimeMS,Alt,Press,Temp" }, \ + "BARO", "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \ { LOG_POWR_MSG, sizeof(log_POWR), \ "POWR","ICCH","TimeMS,Vcc,VServo,Flags" }, \ { LOG_CMD_MSG, sizeof(log_Cmd), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 96500451fd1..a0abbfc1fa1 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -763,6 +763,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro) altitude : baro.get_altitude(), pressure : baro.get_pressure(), temperature : (int16_t)(baro.get_temperature() * 100), + climbrate : baro.get_climb_rate() }; WriteBlock(&pkt, sizeof(pkt)); } From 43c5a70424aa5c2e12145e8e732af5307d6aa7ec Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 20:54:44 +0900 Subject: [PATCH 155/254] Copter: completely disable vel controller --- ArduCopter/control_guided.pde | 7 +++++++ ArduCopter/defines.h | 6 ++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 02deb9d4619..81557c54c54 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -64,6 +64,7 @@ void guided_pos_control_start() set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } +#if NAV_GUIDED == ENABLED // initialise guided mode's velocity controller void guided_vel_control_start() { @@ -77,6 +78,7 @@ void guided_vel_control_start() // initialise velocity controller pos_control.init_vel_controller_xyz(); } +#endif // guided_set_destination - sets guided mode's target destination static void guided_set_destination(const Vector3f& destination) @@ -89,6 +91,7 @@ static void guided_set_destination(const Vector3f& destination) wp_nav.set_wp_destination(destination); } +#if NAV_GUIDED == ENABLED // guided_set_velocity - sets guided mode's target velocity static void guided_set_velocity(const Vector3f& velocity) { @@ -100,6 +103,7 @@ static void guided_set_velocity(const Vector3f& velocity) // set position controller velocity target pos_control.set_desired_velocity(velocity); } +#endif // guided_run - runs the guided controller // should be called at 100hz or more @@ -128,9 +132,12 @@ static void guided_run() guided_pos_control_run(); break; +#if NAV_GUIDED == ENABLED case Guided_Velocity: // run velocity controller guided_vel_control_run(); + break; +#endif } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5e9cdbbe5d4..4c872d27857 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -195,8 +195,10 @@ enum AutoMode { // Guided modes enum GuidedMode { Guided_TakeOff, - Guided_WP, - Guided_Velocity + Guided_WP +#if NAV_GUIDED == ENABLED + ,Guided_Velocity +#endif }; // RTL states From 00f98822417b048fa822bf48c8505946e440a8e8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 20:56:05 +0900 Subject: [PATCH 156/254] Copter: remove DO_MOUNT_CONFIGURE support The functions in the mount lib didn't work anyway --- ArduCopter/commands_logic.pde | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index fafd0959883..384a7a88be7 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -151,16 +151,6 @@ static bool start_command(const AP_Mission::Mission_Command& cmd) break; #endif -#if MOUNT == ENABLED - case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| - camera_mount.configure_cmd(); - break; - - case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| - camera_mount.control_cmd(); - break; -#endif - #if PARACHUTE == ENABLED case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute do_parachute(cmd); From a4da667e2b99477a6655238e83647c8737cae1e4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 20:56:30 +0900 Subject: [PATCH 157/254] Mount: remove CMD_DO_MOUNT_CONFIGURE support --- libraries/AP_Mount/AP_Mount.cpp | 12 ------------ libraries/AP_Mount/AP_Mount.h | 2 -- 2 files changed, 14 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 73a530477d5..67fd26da35c 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -546,18 +546,6 @@ void AP_Mount::set_roi_cmd(const struct Location *target_loc) #endif } -/// Set mount configuration, triggered by mission script commands -void AP_Mount::configure_cmd() -{ - // TODO get the information out of the mission command and use it -} - -/// Control the mount (depends on the previously set mount configuration), triggered by mission script commands -void AP_Mount::control_cmd() -{ - // TODO get the information out of the mission command and use it -} - /// returns the angle (degrees*100) that the RC_Channel input is receiving int32_t AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 7d7829b20f0..2868560920d 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -54,8 +54,6 @@ class AP_Mount void control_msg(mavlink_message_t* msg); void status_msg(mavlink_message_t* msg, mavlink_channel_t chan); void set_roi_cmd(const struct Location *target_loc); - void configure_cmd(); - void control_cmd(); // should be called periodically void update_mount_position(); From 8b87a407edaf572a933de9dab2b88e75949d5f1a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 20:56:49 +0900 Subject: [PATCH 158/254] Copter: remove debug --- ArduCopter/motor_test.pde | 3 --- 1 file changed, 3 deletions(-) diff --git a/ArduCopter/motor_test.pde b/ArduCopter/motor_test.pde index 6d08339cda1..33aadac6419 100644 --- a/ArduCopter/motor_test.pde +++ b/ArduCopter/motor_test.pde @@ -96,9 +96,6 @@ static bool mavlink_motor_test_check(mavlink_channel_t chan) // returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec) { - // debug - cliSerial->printf_P(PSTR("\nMotTest Seq:%d TT:%d Thr:%d TimOut:%4.2f"),(int)motor_seq, (int)throttle_type, (int)throttle_value, (float)timeout_sec); - // if test has not started try to start it if (!ap.motor_test) { // perform checks that it is ok to start test From fdf6aa5492534150b7e42fa98f6f4caaf5e88ac6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 21:03:50 +0900 Subject: [PATCH 159/254] Copter: shorten ESC calibration message --- ArduCopter/radio.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index f4853f3bf17..0a274e1a834 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -62,13 +62,13 @@ static void init_rc_out() // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(1); // display message on console - cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n")); + cliSerial->printf_P(PSTR("Entering ESC Cal: restart APM.\n")); // turn on esc calibration notification AP_Notify::flags.esc_calibration = true; // block until we restart while(1) { delay(5); } }else{ - cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n")); + cliSerial->printf_P(PSTR("ESC Cal: passing throttle through to ESCs.\n")); // clear esc flag g.esc_calibrate.set_and_save(0); // pass through user throttle to escs From 47418b78d6086ce3985b08cee4edd3b59c27f043 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 21:15:02 +0900 Subject: [PATCH 160/254] Relay: reduce num relays to 2 --- libraries/AP_Relay/AP_Relay.cpp | 4 ++++ libraries/AP_Relay/AP_Relay.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 75fe1e175cf..7fb362a4d35 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -42,19 +42,23 @@ const AP_Param::GroupInfo AP_Relay::var_info[] PROGMEM = { // @Values: -1:Disabled,13:APM2 A9 pin,47:APM1 relay,50:Pixhawk FMU AUX1,51:Pixhawk FMU AUX2,52:Pixhawk FMU AUX3,53:Pixhawk FMU AUX4,54:Pixhawk FMU AUX5,55:Pixhawk FMU AUX6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 AP_GROUPINFO("PIN2", 1, AP_Relay, _pin[1], -1), +#if AP_RELAY_NUM_RELAYS > 2 // @Param: PIN3 // @DisplayName: Third Relay Pin // @Description: Digital pin number for 3rd relay control. // @User: Standard // @Values: -1:Disabled,13:APM2 A9 pin,47:APM1 relay,50:Pixhawk FMU AUX1,51:Pixhawk FMU AUX2,52:Pixhawk FMU AUX3,53:Pixhawk FMU AUX4,54:Pixhawk FMU AUX5,55:Pixhawk FMU AUX6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 AP_GROUPINFO("PIN3", 2, AP_Relay, _pin[2], -1), +#endif +#if AP_RELAY_NUM_RELAYS > 3 // @Param: PIN4 // @DisplayName: Fourth Relay Pin // @Description: Digital pin number for 4th relay control. // @User: Standard // @Values: -1:Disabled,13:APM2 A9 pin,47:APM1 relay,50:Pixhawk FMU AUX1,51:Pixhawk FMU AUX2,52:Pixhawk FMU AUX3,53:Pixhawk FMU AUX4,54:Pixhawk FMU AUX5,55:Pixhawk FMU AUX6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 AP_GROUPINFO("PIN4", 3, AP_Relay, _pin[3], -1), +#endif AP_GROUPEND }; diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h index 2aa9a441c5f..fc7b3c6eac9 100644 --- a/libraries/AP_Relay/AP_Relay.h +++ b/libraries/AP_Relay/AP_Relay.h @@ -15,7 +15,7 @@ #include -#define AP_RELAY_NUM_RELAYS 4 +#define AP_RELAY_NUM_RELAYS 2 /// @class AP_Relay /// @brief Class to manage the APM relay From bafd9fd53fdb3ca4df177f7bc8e0e7ed655ce877 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 21:15:15 +0900 Subject: [PATCH 161/254] RangeFinder: reduce num instances to 1 --- libraries/AP_RangeFinder/RangeFinder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index c071d865f68..bef3a9abaf9 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -22,7 +22,7 @@ #include // Maximum number of range finder instances available on this platform -#define RANGEFINDER_MAX_INSTANCES 2 +#define RANGEFINDER_MAX_INSTANCES 1 class AP_RangeFinder_Backend; From 4bd3f593ef5ad433b5a31459541b1e993fc23fd4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 22:18:01 +0900 Subject: [PATCH 162/254] Copter: ReleaseNotes for AC3.2-rc13 --- ArduCopter/ReleaseNotes.txt | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index b0a32f1b0cb..37f7ea596aa 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,21 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc13 23-Oct-2014 +Changes from 3.2-rc12 +1) DCM check triggers LAND if yaw disagrees with GPS by > 60deg (configure with DCM_CHECK_THRESH param) and in Loiter, PosHold, Auto, etc +2) Safety features: + a) landing detector checks baro climbrate between -1.5 ~ +1.5 m/s + b) sanity check AHRS_RP_P and AHRS_YAW_P are never less than 0.05 + c) check set-mode requests from GCS are for this vehicle +3) Bug fixes: + a) fix ch6 tuning of wp-speed (was getting stuck at zero) + b) parachute servo set to off position on startup + c) Auto Takeoff timing bug fix that could cause severe lean on takeoff + d) timer fix for "slow start" of motors on Pixhawk (timer was incorrectly based on 100hz APM2 main loop speed) +4) reduced number of relays from 4 to 2 (saves memory and flash required on APM boards) +5) reduced number of range finders from 2 to 1 (saves memory and flash on APM boards) +6) allow logging from startup when LOG_BITMASK set to "All+DisarmedLogging" +------------------------------------------------------------------ ArduCopter 3.2-rc12 10-Oct-2014 Changes from 3.2-rc11 1) disable sonar on APM1 and TradHeli (APM1 & APM2) to allow code to fit From 83a5102ec5ffbaae80edbe9615a9757e0f99b893 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Oct 2014 22:18:10 +0900 Subject: [PATCH 163/254] Copter: version to AC3.2-rc13 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 2129fc2fc9d..e8e78d027b3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc12" +#define THISFIRMWARE "ArduCopter V3.2-rc13" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 7ad0b6177f4b588988aaac668906e673b2cdf89c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 13 Oct 2014 19:07:38 +1100 Subject: [PATCH 164/254] AP_RangeFinder: auto-update PX4 ll40ls max/min distance this allows the range of the Lidar to be set by the user using RNGFND_MAX_CM and RNGFND_MIN_CM --- libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp | 16 +++++++++++++++- libraries/AP_RangeFinder/AP_RangeFinder_PX4.h | 3 +++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp index ae620f8bd8b..cc29868a4c8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp @@ -39,7 +39,9 @@ uint8_t AP_RangeFinder_PX4::num_px4_instances = 0; already know that we should setup the rangefinder */ AP_RangeFinder_PX4::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_ranger, instance, _state) + AP_RangeFinder_Backend(_ranger, instance, _state), + _last_max_distance_cm(-1), + _last_min_distance_cm(-1) { _fd = open_driver(); @@ -111,6 +113,18 @@ void AP_RangeFinder_PX4::update(void) float sum = 0; uint16_t count = 0; + if (_last_max_distance_cm != ranger._max_distance_cm[state.instance] || + _last_min_distance_cm != ranger._min_distance_cm[state.instance]) { + float max_distance = ranger._max_distance_cm[state.instance]*0.01f; + float min_distance = ranger._min_distance_cm[state.instance]*0.01f; + if (ioctl(_fd, RANGEFINDERIOCSETMAXIUMDISTANCE, (unsigned long)&max_distance) == 0 && + ioctl(_fd, RANGEFINDERIOCSETMINIUMDISTANCE, (unsigned long)&min_distance) == 0) { + _last_max_distance_cm = ranger._max_distance_cm[state.instance]; + _last_min_distance_cm = ranger._min_distance_cm[state.instance]; + } + } + + while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) && range_report.timestamp != _last_timestamp) { // Only take valid readings diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h index f096429c928..48d5fb202e4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h @@ -39,6 +39,9 @@ class AP_RangeFinder_PX4 : public AP_RangeFinder_Backend int _fd; uint64_t _last_timestamp; + int16_t _last_max_distance_cm; + int16_t _last_min_distance_cm; + // we need to keep track of how many PX4 drivers have been loaded // so we can open the right device filename static uint8_t num_px4_instances; From 8041613c06db6fa021e525d9c63187b260061990 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Oct 2014 15:00:47 +0900 Subject: [PATCH 165/254] RangeFinder: PulsedLight I2C addr to 0x62 --- libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 896882bc22a..21e089834b1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -25,7 +25,7 @@ */ // i2c address -#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x42 +#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x62 // min and max distances #define AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE 0 From 3a8d4cdcdab4f0f2924ec829cc09797affee20b1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Oct 2014 12:37:56 +0900 Subject: [PATCH 166/254] Copter: fix to dcm-check to be continuous dcm-check was triggering after 10 bad headings but these did not need to be continuous meaning if the vehicle was flown long enough it would almost certainly trigger a dcm-check failure and land --- ArduCopter/ekf_check.pde | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index 7e2de943b81..da73ac74ea7 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -81,12 +81,12 @@ void ekf_dcm_check() } } } else { - // if compass is flagged as bad - if (ekf_check_state.bad_compass) { - // reduce counter + // reduce counter + if (ekf_check_state.fail_count_compass > 0) { ekf_check_state.fail_count_compass--; - // if counter reaches zero then clear flag - if (ekf_check_state.fail_count_compass == 0) { + + // if compass is flagged as bad and the counter reaches zero then clear flag + if (ekf_check_state.bad_compass && ekf_check_state.fail_count_compass == 0) { ekf_check_state.bad_compass = false; // log recovery in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED); From fae45b29fab15306d65dc9d7a2ee96addf35807b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Aug 2014 19:25:08 +1000 Subject: [PATCH 167/254] AP_InertialSensor: fixed timer bug in HIL sensors --- libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index d50691c4023..bbb5e8d8ee7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -63,7 +63,7 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) if (_sample_available()) { return true; } - uint32_t start = hal.scheduler->micros(); + uint32_t start = hal.scheduler->millis(); while ((hal.scheduler->millis() - start) < timeout_ms) { uint32_t tnow = hal.scheduler->micros(); uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow; From 1921e22265f2cb83c255bff2a99bb4551daf57b3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Oct 2014 22:27:14 +0900 Subject: [PATCH 168/254] Copter: ReleaseNotes for AC3.2-rc14 --- ArduCopter/ReleaseNotes.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 37f7ea596aa..c67227ec65b 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,9 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2-rc14 27-Oct-2014 +Changes from 3.2-rc13 +1) Bug fix to DCM-check to require one continuous second of bad heading before triggering LAND +------------------------------------------------------------------ ArduCopter 3.2-rc13 23-Oct-2014 Changes from 3.2-rc12 1) DCM check triggers LAND if yaw disagrees with GPS by > 60deg (configure with DCM_CHECK_THRESH param) and in Loiter, PosHold, Auto, etc From a9e6c06f1a2a195d5a3a859ea68ed40c2ee4840d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Oct 2014 22:27:27 +0900 Subject: [PATCH 169/254] Copter: update version to AC3.2-rc14 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e8e78d027b3..70a4cdd116c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc13" +#define THISFIRMWARE "ArduCopter V3.2-rc14" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From ef0b934b102f1d9554abcbd20e1bbef80f745214 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 22 Oct 2014 01:39:08 -0700 Subject: [PATCH 170/254] Copter: don't stop logging on disarm when LOG_WHEN_DISARMED is set --- ArduCopter/motors.pde | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index eaa64d43929..de1b08b69c0 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -637,7 +637,9 @@ static void init_disarm_motors() Log_Write_Event(DATA_DISARMED); // suspend logging - DataFlash.EnableWrites(false); + if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) { + DataFlash.EnableWrites(false); + } // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); From 95538d29920a32176d0ae1b7a2b6ce1471e5d1d0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Oct 2014 20:22:48 +0900 Subject: [PATCH 171/254] AHRS: add reset_gyro_drift method --- libraries/AP_AHRS/AP_AHRS.h | 4 ++++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 9 +++++++++ libraries/AP_AHRS/AP_AHRS_DCM.h | 4 ++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 ++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 4 ++++ 5 files changed, 31 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f644c2de929..5e30e51eee9 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -181,6 +181,10 @@ class AP_AHRS // return the current estimate of the gyro drift virtual const Vector3f &get_gyro_drift(void) const = 0; + // reset the current gyro drift estimate + // should be called if gyro offsets are recalculated + virtual void reset_gyro_drift(void) = 0; + // reset the current attitude, used on new IMU calibration virtual void reset(bool recover_eulers=false) = 0; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 04d83ea6bc1..b698bb6c193 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -36,6 +36,15 @@ extern const AP_HAL::HAL& hal; // http://gentlenav.googlecode.com/files/fastRotations.pdf #define SPIN_RATE_LIMIT 20 +// reset the current gyro drift estimate +// should be called if gyro offsets are recalculated +void +AP_AHRS_DCM::reset_gyro_drift(void) +{ + _omega_I.zero(); + _omega_I_sum.zero(); + _omega_I_sum_time = 0; +} // run a full DCM update round void diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 76160c09e54..8833028d3e8 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -75,6 +75,10 @@ class AP_AHRS_DCM : public AP_AHRS return _omega_I; } + // reset the current gyro drift estimate + // should be called if gyro offsets are recalculated + void reset_gyro_drift(void); + // Methods void update(void); void reset(bool recover_eulers = false); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 224e7a9ba7c..8853a9dfd6d 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -50,6 +50,16 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const return _gyro_bias; } +// reset the current gyro drift estimate +// should be called if gyro offsets are recalculated +void AP_AHRS_NavEKF::reset_gyro_drift(void) +{ + // update DCM + AP_AHRS_DCM::reset_gyro_drift(); + + // To-Do: add call to do the same on EKF +} + void AP_AHRS_NavEKF::update(void) { // we need to restore the old DCM attitude values as these are diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index d03ffdc1732..bc065866fa4 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -49,6 +49,10 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM // return the current drift correction integrator value const Vector3f &get_gyro_drift(void) const; + // reset the current gyro drift estimate + // should be called if gyro offsets are recalculated + void reset_gyro_drift(void); + void update(void); void reset(bool recover_eulers = false); From 7b4cd9ee371a2abf7943d7eaaf1ca7a768060877 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Oct 2014 20:23:18 +0900 Subject: [PATCH 172/254] Copter: reset ahrs gyro drift after gyro calibration --- ArduCopter/GCS_Mavlink.pde | 2 ++ ArduCopter/system.pde | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index c4a428176cd..d7930ccbb99 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1113,6 +1113,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1) { // gyro offset calibration ins.init_gyro(); + // reset ahrs gyro bias + ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } if (packet.param3 == 1) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 4ab9ff34872..4e34d8cb4ef 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -320,6 +320,11 @@ static void startup_ground(bool force_gyro_cal) report_ins(); #endif + // reset ahrs gyro bias + if (force_gyro_cal) { + ahrs.reset_gyro_drift(); + } + // setup fast AHRS gains to get right attitude ahrs.set_fast_gains(true); From a8a6368f05c885d395f8bbd987c8669bb55e46a1 Mon Sep 17 00:00:00 2001 From: Benoit PEREIRA DA SILVA Date: Sun, 21 Sep 2014 14:48:35 +0200 Subject: [PATCH 173/254] GPS: use primary for Notification --- libraries/AP_GPS/AP_GPS.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index adbcfcad66b..28130fffa2d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -334,9 +334,6 @@ AP_GPS::update(void) update_instance(i); } - // update notify with gps status. We always base this on the first GPS - AP_Notify::flags.gps_status = state[0].status; - #if GPS_MAX_INSTANCES > 1 // work out which GPS is the primary, and how many sensors we have for (uint8_t i=0; i Date: Wed, 29 Oct 2014 12:02:59 +0900 Subject: [PATCH 174/254] GPS: init primary_instance to zero --- libraries/AP_GPS/AP_GPS.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 28130fffa2d..414986f0aae 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -72,8 +72,8 @@ void AP_GPS::init(DataFlash_Class *dataflash) { _DataFlash = dataflash; hal.uartB->begin(38400UL, 256, 16); -#if GPS_MAX_INSTANCES > 1 primary_instance = 0; +#if GPS_MAX_INSTANCES > 1 if (hal.uartE != NULL) { hal.uartE->begin(38400UL, 256, 16); } @@ -363,7 +363,6 @@ AP_GPS::update(void) } } #else - primary_instance=0; num_instances = 1; #endif // GPS_MAX_INSTANCES // update notify with gps status. We always base this on the primary_instance From d04a740bcb01b31153dd4d7e377d47364a210688 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 29 Oct 2014 12:20:43 +1100 Subject: [PATCH 175/254] AP_AHRS : Add reset of EKF gyro bias states --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 8853a9dfd6d..60f384e8823 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -57,7 +57,8 @@ void AP_AHRS_NavEKF::reset_gyro_drift(void) // update DCM AP_AHRS_DCM::reset_gyro_drift(); - // To-Do: add call to do the same on EKF + // reset the EKF gyro bias states + EKF.resetGyroBias(); } void AP_AHRS_NavEKF::update(void) From a7caa91ceffd4f25dd982550f2c6f5aa07abe4fb Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 29 Oct 2014 12:21:42 +1100 Subject: [PATCH 176/254] AP_NavEKF : Add public method to reset gyro bias states --- libraries/AP_NavEKF/AP_NavEKF.cpp | 12 ++++++++++++ libraries/AP_NavEKF/AP_NavEKF.h | 3 +++ 2 files changed, 15 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5fde16ab419..42e03aaa389 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2810,6 +2810,18 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const gyroBias = state.gyro_bias / dtIMU; } +// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances +void NavEKF::resetGyroBias(void) +{ + state.gyro_bias.zero(); + zeroRows(P,10,12); + zeroCols(P,10,12); + P[10][10] = sq(radians(0.1f * dtIMU)); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + +} + // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 void NavEKF::getAccelBias(Vector3f &accelBias) const { diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index ba27b1bfbe9..51b8a9a5e87 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -105,6 +105,9 @@ class NavEKF // return body axis gyro bias estimates in rad/sec void getGyroBias(Vector3f &gyroBias) const; + // reset body axis gyro bias estimates + void resetGyroBias(void); + // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 void getAccelBias(Vector3f &accelBias) const; From dc3509ef5523bd214d961583239da93279d7b9a7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Oct 2014 15:19:10 +0900 Subject: [PATCH 177/254] Copter: fail to arm if gyro cal fails --- ArduCopter/GCS_Mavlink.pde | 6 +++++- ArduCopter/motors.pde | 20 +++++++++++++++++--- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d7930ccbb99..c7fb2c42914 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1159,8 +1159,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // run pre_arm_checks and arm_checks and display failures pre_arm_checks(true); if(ap.pre_arm_check && arm_checks(true)) { - init_arm_motors(); + if (init_arm_motors()) { result = MAV_RESULT_ACCEPTED; + } else { + AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false + result = MAV_RESULT_UNSUPPORTED; + } }else{ AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false result = MAV_RESULT_UNSUPPORTED; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index de1b08b69c0..9e736b0052d 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -59,7 +59,11 @@ static void arm_motors_check() // run pre-arm-checks and display failures pre_arm_checks(true); if(ap.pre_arm_check && arm_checks(true)) { - init_arm_motors(); + if (!init_arm_motors()) { + // reset arming counter if arming fail + arming_counter = 0; + AP_Notify::flags.arming_failed = true; + } }else{ // reset arming counter if pre-arm checks fail arming_counter = 0; @@ -120,7 +124,8 @@ static void auto_disarm_check() } // init_arm_motors - performs arming process including initialisation of barometer and gyros -static void init_arm_motors() +// returns false in the unlikely case that arming fails (because of a gyro calibration failure) +static bool init_arm_motors() { // arming marker // Flag used to track if we have armed the motors the first time. @@ -167,8 +172,14 @@ static void init_arm_motors() } if(did_ground_start == false) { - did_ground_start = true; startup_ground(true); + // final check that gyros calibrated successfully + if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro cal failed")); + AP_Notify::flags.armed = false; + return false; + } + did_ground_start = true; } // fast baro calibration to reset ground pressure @@ -219,6 +230,9 @@ static void init_arm_motors() // reenable failsafe failsafe_enable(); + + // return success + return true; } // perform pre-arm checks and set ap.pre_arm_check flag From 683f3a55c40a790bf03360e1e3de76adb3d93415 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Oct 2014 15:59:22 +0900 Subject: [PATCH 178/254] Copter: re-enable CPU failsafe if arming fails --- ArduCopter/motors.pde | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 9e736b0052d..80217627f9f 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -177,8 +177,9 @@ static bool init_arm_motors() if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro cal failed")); AP_Notify::flags.armed = false; + failsafe_enable(); return false; - } + } did_ground_start = true; } From ddb8796d2c0d38c41b7c5245e73c0517c0532c43 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Oct 2014 15:59:54 +0900 Subject: [PATCH 179/254] Copter: return false when arming fails --- ArduCopter/motors.pde | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 80217627f9f..6147efa3b24 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -205,8 +205,7 @@ static bool init_arm_motors() motors.output_min(); failsafe_enable(); AP_Notify::flags.armed = false; - AP_Notify::flags.arming_failed = false; - return; + return false; } #if SPRAYER == ENABLED From fecad463367986eb511bc1c18fbe095a3af5bf52 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 29 Oct 2014 19:32:57 +1100 Subject: [PATCH 180/254] AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition --- libraries/AP_NavEKF/AP_NavEKF.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 42e03aaa389..017789f5c59 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -102,6 +102,9 @@ extern const AP_HAL::HAL& hal; // assume 3m/s to start #define STARTUP_WIND_SPEED 3.0f +// initial gyro bias uncertainty (deg/sec) +#define INIT_GYRO_BIAS_UNCERTAINTY 0.1f + // Define tuning parameters const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -2816,7 +2819,7 @@ void NavEKF::resetGyroBias(void) state.gyro_bias.zero(); zeroRows(P,10,12); zeroCols(P,10,12); - P[10][10] = sq(radians(0.1f * dtIMU)); + P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; @@ -2938,7 +2941,7 @@ void NavEKF::CovarianceInit() P[8][8] = P[7][7]; P[9][9] = sq(5.0f); // delta angle biases - P[10][10] = sq(radians(0.1f * dtIMU)); + P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU)); P[11][11] = P[10][10]; P[12][12] = P[10][10]; // Z delta velocity bias From 3a292db7f10a89278d6b6f725fe36793bac86415 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Oct 2014 18:09:11 +0900 Subject: [PATCH 181/254] Copter: update AC3.2-rc14 ReleaseNotes --- ArduCopter/ReleaseNotes.txt | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index c67227ec65b..07bed45ef94 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,8 +1,14 @@ ArduCopter Release Notes: ------------------------------------------------------------------ -ArduCopter 3.2-rc14 27-Oct-2014 +ArduCopter 3.2-rc14 30-Oct-2014 Changes from 3.2-rc13 -1) Bug fix to DCM-check to require one continuous second of bad heading before triggering LAND +1) Safety Features: + a) fail to arm if second gyro calibration fails (can be disabled with ARMING_CHECK) +2) Bug fixes: + a) DCM-check to require one continuous second of bad heading before triggering LAND + b) I2C bug that could lead to Pixhawk freezing up if I2C bus is noisy + c) reset DCM and EKF gyro bias estimates after gyro calibration (DCM heading could drift after takeoff due to sudden change in gyro values) + d) use primary GPS for LED status (instead of always using first GPS) ------------------------------------------------------------------ ArduCopter 3.2-rc13 23-Oct-2014 Changes from 3.2-rc12 From bd69290e5af7e0d7ab93428c8e3696983c92cbdd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 31 Oct 2014 10:56:46 +0900 Subject: [PATCH 182/254] Copter: minor update to AC3.2-rc14 ReleaseNotes --- ArduCopter/ReleaseNotes.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 07bed45ef94..a1025051612 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,6 @@ ArduCopter Release Notes: ------------------------------------------------------------------ -ArduCopter 3.2-rc14 30-Oct-2014 +ArduCopter 3.2-rc14 31-Oct-2014 Changes from 3.2-rc13 1) Safety Features: a) fail to arm if second gyro calibration fails (can be disabled with ARMING_CHECK) @@ -8,7 +8,7 @@ Changes from 3.2-rc13 a) DCM-check to require one continuous second of bad heading before triggering LAND b) I2C bug that could lead to Pixhawk freezing up if I2C bus is noisy c) reset DCM and EKF gyro bias estimates after gyro calibration (DCM heading could drift after takeoff due to sudden change in gyro values) - d) use primary GPS for LED status (instead of always using first GPS) + d) use primary GPS for LED status (instead of always using first GPS) ------------------------------------------------------------------ ArduCopter 3.2-rc13 23-Oct-2014 Changes from 3.2-rc12 From f8de0ecc9e0ad09fc6edac5518d7922b420208ac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Nov 2014 13:49:25 +0900 Subject: [PATCH 183/254] Copter: update AC3.2 ReleaseNotes --- ArduCopter/ReleaseNotes.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index a1025051612..5b7beac9c96 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,6 @@ ArduCopter Release Notes: ------------------------------------------------------------------ -ArduCopter 3.2-rc14 31-Oct-2014 +ArduCopter 3.2 07-Nov2014 / 3.2-rc14 31-Oct-2014 Changes from 3.2-rc13 1) Safety Features: a) fail to arm if second gyro calibration fails (can be disabled with ARMING_CHECK) From c8e0f3e13a7c025eae9eacd8783bac7064e17bd3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Nov 2014 13:49:50 +0900 Subject: [PATCH 184/254] Copter: update version to AC3.2 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 70a4cdd116c..e977c955fac 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2-rc14" +#define THISFIRMWARE "ArduCopter V3.2" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 0a3ef326493598be5838bb34f2189b9ce11438d8 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Tue, 29 Jul 2014 10:54:58 +0200 Subject: [PATCH 185/254] VRBRAIN: modified target clean --- mk/vrbrain_targets.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mk/vrbrain_targets.mk b/mk/vrbrain_targets.mk index 956eaf64417..6b0ea5aa50e 100644 --- a/mk/vrbrain_targets.mk +++ b/mk/vrbrain_targets.mk @@ -145,7 +145,9 @@ vrhero-v10: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrhero-v10.export $ #vrbrain: vrbrain-v40 vrbrain-v45 vrbrain-v50 vrbrain-v51 vrubrain-v51 vrhero-v10 vrbrain: vrbrain-v45 vrbrain-v51 vrubrain-v51 -vrbrain-clean: clean vrbrain-archives-clean +vrbrain-clean: clean vrbrain-build-clean vrbrain-archives-clean + +vrbrain-build-clean: $(v) /bin/rm -rf $(VRBRAIN_ROOT)/makefiles/build $(VRBRAIN_ROOT)/Build vrbrain-cleandep: clean From 2de18f844b566ac88ff05c7e20ba0bc2da672db6 Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Thu, 14 Aug 2014 08:18:39 +0200 Subject: [PATCH 186/254] AP_Compass: VRBRAIN. Deal with external mag connected on internal I2C on VRBRAIN 4.5 This enables user to set the external parameter to true even if only one compass is connected --- libraries/AP_Compass/AP_Compass_VRBRAIN.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp index fe42a639294..7f05388a668 100644 --- a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp +++ b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp @@ -57,10 +57,8 @@ bool AP_Compass_VRBRAIN::init(void) } for (uint8_t i=0; i<_num_instances; i++) { -#ifdef DEVIOCGDEVICEID // get device id _dev_id[i] = ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0); -#endif // average over up to 20 samples if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) { @@ -70,6 +68,11 @@ bool AP_Compass_VRBRAIN::init(void) // remember if the compass is external _is_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0); +#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) + //deal with situations where user has cut internal mag on VRBRAIN 4.5 + //and uses only one external mag attached to the internal I2C bus + _is_external[i] = _external.load() ? _external.get() : _is_external[i]; +#endif if (_is_external[i]) { hal.console->printf("Using external compass[%u]\n", (unsigned)i); } @@ -109,7 +112,7 @@ bool AP_Compass_VRBRAIN::read(void) _sum[i].rotate(MAG_BOARD_ORIENTATION); // override any user setting of COMPASS_EXTERNAL - _external.set(_is_external[0]); + //_external.set(_is_external[0]); if (_is_external[i]) { // add user selectable orientation From 1455d700d456af7c9d95583f3d67c3120a70f527 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Thu, 11 Sep 2014 11:47:58 +0200 Subject: [PATCH 187/254] VRBRAIN: defined AirSpeed inputs for ArduPlane on VR Micro Brain 5 --- libraries/AP_Airspeed/AP_Airspeed.cpp | 4 ++++ libraries/AP_HAL_VRBRAIN/AnalogIn.cpp | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 2878497bf37..4dce30f4cdd 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -60,7 +60,11 @@ extern const AP_HAL::HAL& hal; #elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) #define ARSPD_DEFAULT_PIN 0 #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + #define ARSPD_DEFAULT_PIN 1 +#else #define ARSPD_DEFAULT_PIN 0 +#endif #elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) #define ARSPD_DEFAULT_PIN 0 #endif diff --git a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp index e4e5bd36604..9dd8f984226 100644 --- a/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp +++ b/libraries/AP_HAL_VRBRAIN/AnalogIn.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #define ANLOGIN_DEBUGGING 0 @@ -57,6 +58,11 @@ static const struct { { 10, 3.3f/4096 }, { 11, 3.3f/4096 }, #elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + { 1, 3.3f/4096 }, + { 2, 3.3f/4096 }, + { 3, 3.3f/4096 }, +#endif { 10, 3.3f/4096 }, #elif defined(CONFIG_ARCH_BOARD_VRHERO_V10) { 10, 3.3f/4096 }, From 12ade1e65ca2ba5061e0e686eada5ec31bbe4828 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Thu, 11 Sep 2014 11:48:49 +0200 Subject: [PATCH 188/254] VRBRAIN: added flag for to build versions with different kind of RC Inputs --- mk/vrbrain_targets.mk | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mk/vrbrain_targets.mk b/mk/vrbrain_targets.mk index 6b0ea5aa50e..350cb20b8ff 100644 --- a/mk/vrbrain_targets.mk +++ b/mk/vrbrain_targets.mk @@ -30,7 +30,7 @@ endif - +RC_INPUTS_TYPE := # we have different config files for vrbrain_v40, vrbrain_v45, vrbrain_v50, vrbrain_v51, vrubrain_v51 and vrhero_v10 VRBRAIN_MK_DIR=$(SRCROOT)/$(MK_DIR)/VRBRAIN @@ -41,6 +41,10 @@ VRBRAIN_VB51_CONFIG_FILE=config_vrbrain-v51_APM.mk VRBRAIN_VU51_CONFIG_FILE=config_vrubrain-v51_APM.mk VRBRAIN_VH10_CONFIG_FILE=config_vrhero-v10_APM.mk +ifneq ($(findstring PWM, $(RC_INPUTS_TYPE)),) +EXTRAFLAGS += "-DCONFIG_RC_INPUTS=RC_INPUT_PWM " +endif + SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_VRBRAIN -DSKETCHNAME="\\\"$(SKETCH)\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH) WARNFLAGS = -Wno-psabi -Wno-packed From f50d48f40976b07a1677341bfb0ea3b3a4a51917 Mon Sep 17 00:00:00 2001 From: LukeMike Date: Fri, 12 Sep 2014 08:40:47 +0200 Subject: [PATCH 189/254] VRBRAIN: added new board VR Brain 5 PRO --- mk/VRBRAIN/config_vrbrain-v51Pro_APM.mk | 172 ++++++++++++++++++++++++ mk/vrbrain_targets.mk | 123 ++++++++++++++++- 2 files changed, 292 insertions(+), 3 deletions(-) create mode 100644 mk/VRBRAIN/config_vrbrain-v51Pro_APM.mk diff --git a/mk/VRBRAIN/config_vrbrain-v51Pro_APM.mk b/mk/VRBRAIN/config_vrbrain-v51Pro_APM.mk new file mode 100644 index 00000000000..f659ff3fe2a --- /dev/null +++ b/mk/VRBRAIN/config_vrbrain-v51Pro_APM.mk @@ -0,0 +1,172 @@ +# +# Makefile for the VRBRAIN 5.1 APM configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(SKETCHBOOK)/mk/VRBRAIN/ROMFS_VRBRAIN51_APM + +MODULES += $(APM_MODULE_DIR) + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/buzzer + + +MODULES += drivers/boards/vrbrain-v51Pro +MODULES += drivers/vrbrain/vroutput +MODULES += drivers/vrbrain/vrinput/controls +MODULES += drivers/vrbrain/vrinput + + + + +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/hmc5983 + +MODULES += drivers/ms5611 + + + + + + + + + + + + + + + + +# +# System commands +# +MODULES += systemcmds/mtd +MODULES += systemcmds/bl_update + + + + + +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top + + +MODULES += systemcmds/tests + +MODULES += systemcmds/nshterm + +# +# General system control +# + + + + + + +# +# Estimation modules (EKF/ SO3 / other filters) +# + + + + + + +# +# Vehicle Control +# + + + + + + + + +# +# Logging +# + + +# +# Unit tests +# + + + +# +# Library modules +# +MODULES += modules/systemlib + + +MODULES += modules/uORB + +# +# Libraries +# + + +MODULES += lib/mathlib/math/filter + + + +MODULES += lib/conversion + + + + + + + + + + + + + + + + + + + + + + + + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) \ + $(call _B, msconn, , 2048, msconn_main ) \ + $(call _B, msdis, , 2048, msdis_main ) \ + $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/mk/vrbrain_targets.mk b/mk/vrbrain_targets.mk index 350cb20b8ff..ca789a8083f 100644 --- a/mk/vrbrain_targets.mk +++ b/mk/vrbrain_targets.mk @@ -34,11 +34,18 @@ RC_INPUTS_TYPE := # we have different config files for vrbrain_v40, vrbrain_v45, vrbrain_v50, vrbrain_v51, vrubrain_v51 and vrhero_v10 VRBRAIN_MK_DIR=$(SRCROOT)/$(MK_DIR)/VRBRAIN + VRBRAIN_VB40_CONFIG_FILE=config_vrbrain-v40_APM.mk VRBRAIN_VB45_CONFIG_FILE=config_vrbrain-v45_APM.mk + VRBRAIN_VB50_CONFIG_FILE=config_vrbrain-v50_APM.mk VRBRAIN_VB51_CONFIG_FILE=config_vrbrain-v51_APM.mk + +VRBRAIN_VB51PRO_CONFIG_FILE=config_vrbrain-v51Pro_APM.mk + VRBRAIN_VU51_CONFIG_FILE=config_vrubrain-v51_APM.mk + + VRBRAIN_VH10_CONFIG_FILE=config_vrhero-v10_APM.mk ifneq ($(findstring PWM, $(RC_INPUTS_TYPE)),) @@ -104,6 +111,20 @@ vrbrain-v50: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrbrain-v50.export $(v) cp $(VRBRAIN_ROOT)/Images/vrbrain-v50_APM.bin $(SKETCH)-vrbrain-v50.bin $(v) echo "VRBRAIN $(SKETCH) Firmware is in $(SKETCH)-vrbrain-v50.vrx" + + + + + + + + + + + + + + vrbrain-v51: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrbrain-v51.export $(SKETCHCPP) module_mk $(RULEHDR) $(v) rm -f $(VRBRAIN_ROOT)/makefiles/$(VRBRAIN_VB51_CONFIG_FILE) @@ -118,6 +139,48 @@ vrbrain-v51: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrbrain-v51.export $(v) cp $(VRBRAIN_ROOT)/Images/vrbrain-v51_APM.bin $(SKETCH)-vrbrain-v51.bin $(v) echo "VRBRAIN $(SKETCH) Firmware is in $(SKETCH)-vrbrain-v51.vrx" + + + + + + + + + + + + + + +vrbrain-v51Pro: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrbrain-v51Pro.export $(SKETCHCPP) module_mk + $(RULEHDR) + $(v) rm -f $(VRBRAIN_ROOT)/makefiles/$(VRBRAIN_VB51PRO_CONFIG_FILE) + $(v) cp $(VRBRAIN_MK_DIR)/$(VRBRAIN_VB51PRO_CONFIG_FILE) $(VRBRAIN_ROOT)/makefiles/ + $(v) $(VRBRAIN_MAKE) vrbrain-v51Pro_APM + $(v) rm -f $(VRBRAIN_ROOT)/makefiles/$(VRBRAIN_VB51PRO_CONFIG_FILE) + $(v) rm -f $(SKETCH)-vrbrain-v51Pro.vrx + $(v) rm -f $(SKETCH)-vrbrain-v51Pro.hex + $(v) rm -f $(SKETCH)-vrbrain-v51Pro.bin + $(v) cp $(VRBRAIN_ROOT)/Images/vrbrain-v51Pro_APM.vrx $(SKETCH)-vrbrain-v51Pro.vrx + $(v) cp $(VRBRAIN_ROOT)/Images/vrbrain-v51Pro_APM.hex $(SKETCH)-vrbrain-v51Pro.hex + $(v) cp $(VRBRAIN_ROOT)/Images/vrbrain-v51Pro_APM.bin $(SKETCH)-vrbrain-v51Pro.bin + $(v) echo "VRBRAIN $(SKETCH) Firmware is in $(SKETCH)-vrbrain-v51Pro.vrx" + + + + + + + + + + + + + + + vrubrain-v51: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrubrain-v51.export $(SKETCHCPP) module_mk $(RULEHDR) $(v) rm -f $(VRBRAIN_ROOT)/makefiles/$(VRBRAIN_VU51_CONFIG_FILE) @@ -132,6 +195,20 @@ vrubrain-v51: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrubrain-v51.expo $(v) cp $(VRBRAIN_ROOT)/Images/vrubrain-v51_APM.bin $(SKETCH)-vrubrain-v51.bin $(v) echo "MICRO VRBRAIN $(SKETCH) Firmware is in $(SKETCH)-vrubrain-v51.vrx" + + + + + + + + + + + + + + vrhero-v10: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrhero-v10.export $(SKETCHCPP) module_mk $(RULEHDR) $(v) rm -f $(VRBRAIN_ROOT)/makefiles/$(VRBRAIN_VH10_CONFIG_FILE) @@ -146,10 +223,15 @@ vrhero-v10: $(BUILDROOT)/make.flags $(VRBRAIN_ROOT)/Archives/vrhero-v10.export $ $(v) cp $(VRBRAIN_ROOT)/Images/vrhero-v10_APM.bin $(SKETCH)-vrhero-v10.bin $(v) echo "VRBRAIN $(SKETCH) Firmware is in $(SKETCH)-vrhero-v10.vrx" -#vrbrain: vrbrain-v40 vrbrain-v45 vrbrain-v50 vrbrain-v51 vrubrain-v51 vrhero-v10 -vrbrain: vrbrain-v45 vrbrain-v51 vrubrain-v51 +vrbrainStd: vrbrain-v45 vrbrain-v51 vrubrain-v51 + +vrbrainPro: vrbrain-v51Pro + -vrbrain-clean: clean vrbrain-build-clean vrbrain-archives-clean +vrbrain: vrbrainStd vrbrainPro + +#vrbrain-clean: clean vrbrain-build-clean vrbrain-archives-clean +vrbrain-clean: clean vrbrain-build-clean vrbrain-build-clean: $(v) /bin/rm -rf $(VRBRAIN_ROOT)/makefiles/build $(VRBRAIN_ROOT)/Build @@ -165,6 +247,10 @@ vrbrain-v45-upload: vrbrain-v45 $(RULEHDR) $(v) $(VRBRAIN_MAKE) vrbrain-v45_APM upload + + + + vrbrain-v50-upload: vrbrain-v50 $(RULEHDR) $(v) $(VRBRAIN_MAKE) vrbrain-v50_APM upload @@ -173,10 +259,26 @@ vrbrain-v51-upload: vrbrain-v51 $(RULEHDR) $(v) $(VRBRAIN_MAKE) vrbrain-v51_APM upload + + + + +vrbrain-v51Pro-upload: vrbrain-v51Pro + $(RULEHDR) + $(v) $(VRBRAIN_MAKE) vrbrain-v51Pro_APM upload + + + + + vrubrain-v51-upload: vrubrain-v51 $(RULEHDR) $(v) $(VRBRAIN_MAKE) vrubrain-v51_APM upload + + + + vrhero-v10-upload: vrhero-v10 $(RULEHDR) $(v) $(VRBRAIN_MAKE) vrhero-v10_APM upload @@ -192,15 +294,30 @@ $(VRBRAIN_ROOT)/Archives/vrbrain-v40.export: $(VRBRAIN_ROOT)/Archives/vrbrain-v45.export: $(v) $(VRBRAIN_MAKE_ARCHIVES) + + + $(VRBRAIN_ROOT)/Archives/vrbrain-v50.export: $(v) $(VRBRAIN_MAKE_ARCHIVES) $(VRBRAIN_ROOT)/Archives/vrbrain-v51.export: $(v) $(VRBRAIN_MAKE_ARCHIVES) + + + +$(VRBRAIN_ROOT)/Archives/vrbrain-v51Pro.export: + $(v) $(VRBRAIN_MAKE_ARCHIVES) + + + + $(VRBRAIN_ROOT)/Archives/vrubrain-v51.export: $(v) $(VRBRAIN_MAKE_ARCHIVES) + + + $(VRBRAIN_ROOT)/Archives/vrhero-v10.export: $(v) $(VRBRAIN_MAKE_ARCHIVES) From 45e3b0e9cddd08215b2dd3bc31bf4f95811f429e Mon Sep 17 00:00:00 2001 From: LukeMike Date: Fri, 12 Sep 2014 08:41:47 +0200 Subject: [PATCH 190/254] VRBRAIN: added script for build all VirtualRobotix binaries --- Tools/scripts/build_vrbrain_binaries.sh | 277 ++++++++++++++++++++++++ 1 file changed, 277 insertions(+) create mode 100644 Tools/scripts/build_vrbrain_binaries.sh diff --git a/Tools/scripts/build_vrbrain_binaries.sh b/Tools/scripts/build_vrbrain_binaries.sh new file mode 100644 index 00000000000..0d7901aaa77 --- /dev/null +++ b/Tools/scripts/build_vrbrain_binaries.sh @@ -0,0 +1,277 @@ +#!/bin/bash +# script to build the latest binaries for each vehicle type, ready to upload +# Andrew Tridgell, March 2013 + +export PATH=$PATH:/bin:/usr/bin + +export TMPDIR=$PWD/build.tmp.$$ +echo $TMDIR +rm -rf $TMPDIR +echo "Building in $TMPDIR" + +date +git checkout master +githash=$(git rev-parse HEAD) + +hdate=$(date +"%Y-%m/%Y-%m-%d-%H:%m") +mkdir -p binaries/$hdate +binaries=$PWD/../buildlogs/binaries + +error_count=0 + +. config.mk + +# checkout the right version of the tree +checkout() { + branch="$1" + git stash + if [ "$branch" = "master" ]; then + vbranch="master" + vbranch2="master" + fi + if [ "$branch" = "for_merge" ]; then + vbranch="for_merge" + vbranch2="for_merge" + fi + if [ "$branch" = "for_merge-3.2" ]; then + vbranch="for_merge-3.2" + vbranch2="for_merge" + fi + if [ "$branch" = "tone_alarm" ]; then + vbranch="ToneAlarm" + vbranch2="ToneAlarm" + fi + if [ "$branch" = "tone_alarm-3.2" ]; then + vbranch="ToneAlarm-3.2" + vbranch2="ToneAlarm" + fi + + echo "Checkout with branch $branch" + + git remote update + git checkout -B "$vbranch" remotes/origin/"$vbranch" + git pull -v --progress "origin" "$vbranch" || return 1 + + git log -1 + + pushd ../../vrbrain_nuttx + git remote update + git checkout -B "$vbranch2" remotes/origin/"$vbranch2" + git pull -v --progress "origin" "$vbranch2" || { + popd + return 1 + } + git log -1 + popd + + return 0 +} + +# check if we should skip this build because we have already +# built this version +skip_build() { + [ "$FORCE_BUILD" = "1" ] && return 1 + tag="$1" + ddir="$2" + bname=$(basename $ddir) + ldir=$(dirname $(dirname $(dirname $ddir)))/$tag/$bname + [ -d "$ldir" ] || { + echo "$ldir doesn't exist - building" + return 1 + } + oldversion=$(cat "$ldir/git-version.txt" | head -1) + newversion=$(git log -1 | head -1) + [ "$oldversion" = "$newversion" ] && { + echo "Skipping build - version match $newversion" + return 0 + } + echo "$ldir needs rebuild" + return 1 +} + +addfwversion() { + destdir="$1" + git log -1 > "$destdir/git-version.txt" + [ -f APM_Config.h ] && { + version=$(grep 'define.THISFIRMWARE' *.pde 2> /dev/null | cut -d'"' -f2) + echo >> "$destdir/git-version.txt" + echo "APMVERSION: $version" >> "$destdir/git-version.txt" + } +} + +# copy the built firmware to the right directory +copyit() { + file="$1" + dir="$2" + tag="$3" + bname=$(basename $dir) + tdir=$(dirname $(dirname $(dirname $dir)))/$tag/$bname + if [ "$tag" = "latest" ]; then + mkdir -p "$dir" + /bin/cp "$file" "$dir" + addfwversion "$dir" + fi + echo "Copying $file to $tdir" + mkdir -p "$tdir" + addfwversion "$tdir" + + rsync "$file" "$tdir" +} + +# build plane binaries +build_arduplane() { + tag="$1" + echo "Building ArduPlane $tag binaries" + pushd ArduPlane + test -n "$VRBRAIN_ROOT" && { + echo "Building ArduPlane VRBRAIN binaries" + ddir=$binaries/Plane/$hdate/VRX + checkout $tag || { + echo "Failed checkout of ArduPlane VRBRAIN $tag" + error_count=$((error_count+1)) + continue + } + skip_build $tag $ddir || { + make vrbrain-clean && + make vrbrain || { + echo "Failed build of ArduPlane VRBRAIN $tag" + error_count=$((error_count+1)) + continue + } + copyit ArduPlane-vrbrain-v45.vrx $ddir $tag && + copyit ArduPlane-vrbrain-v45.bin $ddir $tag && + copyit ArduPlane-vrbrain-v45.hex $ddir $tag && + copyit ArduPlane-vrbrain-v51.vrx $ddir $tag && + copyit ArduPlane-vrbrain-v51.bin $ddir $tag && + copyit ArduPlane-vrbrain-v51.hex $ddir $tag && + copyit ArduPlane-vrbrain-v51P.vrx $ddir $tag && + copyit ArduPlane-vrbrain-v51P.bin $ddir $tag && + copyit ArduPlane-vrbrain-v51P.hex $ddir $tag && + copyit ArduPlane-vrbrain-v51Pro.vrx $ddir $tag && + copyit ArduPlane-vrbrain-v51Pro.bin $ddir $tag && + copyit ArduPlane-vrbrain-v51Pro.hex $ddir $tag && + copyit ArduPlane-vrbrain-v51ProP.vrx $ddir $tag && + copyit ArduPlane-vrbrain-v51ProP.bin $ddir $tag && + copyit ArduPlane-vrbrain-v51ProP.hex $ddir $tag && + copyit ArduPlane-vrubrain-v51.vrx $ddir $tag && + copyit ArduPlane-vrubrain-v51.bin $ddir $tag && + copyit ArduPlane-vrubrain-v51.hex $ddir $tag && + copyit ArduPlane-vrubrain-v51P.vrx $ddir $tag && + copyit ArduPlane-vrubrain-v51P.bin $ddir $tag && + copyit ArduPlane-vrubrain-v51P.hex $ddir $tag + } + } + checkout "master" + popd +} + +# build copter binaries +build_arducopter() { + tag="$1" + echo "Building ArduCopter $tag binaries from $(pwd)" + pushd ArduCopter + frames="quad tri hexa y6 octa octa-quad heli" + test -n "$VRBRAIN_ROOT" && { + checkout $tag || { + echo "Failed checkout of ArduCopter VRBRAIN $tag" + error_count=$((error_count+1)) + checkout "master" + popd + return + } + make vrbrain-clean || return + for f in $frames quad-hil heli-hil; do + echo "Building ArduCopter VRBRAIN-$f binaries" + ddir="$binaries/Copter/$hdate/VRX-$f" + skip_build $tag $ddir && continue + rm -rf ../Build.ArduCopter + make vrbrain-$f || { + echo "Failed build of ArduCopter VRBRAIN $tag" + error_count=$((error_count+1)) + continue + } + copyit ArduCopter-vrbrain-v45.vrx $ddir $tag && + copyit ArduCopter-vrbrain-v45.bin $ddir $tag && + copyit ArduCopter-vrbrain-v45.hex $ddir $tag && + copyit ArduCopter-vrbrain-v51.vrx $ddir $tag && + copyit ArduCopter-vrbrain-v51.bin $ddir $tag && + copyit ArduCopter-vrbrain-v51.hex $ddir $tag && + copyit ArduCopter-vrbrain-v51P.vrx $ddir $tag && + copyit ArduCopter-vrbrain-v51P.bin $ddir $tag && + copyit ArduCopter-vrbrain-v51P.hex $ddir $tag && + copyit ArduCopter-vrbrain-v51Pro.vrx $ddir $tag && + copyit ArduCopter-vrbrain-v51Pro.bin $ddir $tag && + copyit ArduCopter-vrbrain-v51Pro.hex $ddir $tag && + copyit ArduCopter-vrbrain-v51ProP.vrx $ddir $tag && + copyit ArduCopter-vrbrain-v51ProP.bin $ddir $tag && + copyit ArduCopter-vrbrain-v51ProP.hex $ddir $tag && + copyit ArduCopter-vrubrain-v51.vrx $ddir $tag && + copyit ArduCopter-vrubrain-v51.bin $ddir $tag && + copyit ArduCopter-vrubrain-v51.hex $ddir $tag && + copyit ArduCopter-vrubrain-v51P.vrx $ddir $tag && + copyit ArduCopter-vrubrain-v51P.bin $ddir $tag && + copyit ArduCopter-vrubrain-v51P.hex $ddir $tag + done + } + checkout "master" + popd +} + +# build rover binaries +build_rover() { + tag="$1" + echo "Building APMrover2 $tag binaries from $(pwd)" + pushd APMrover2 + test -n "$VRBRAIN_ROOT" && { + echo "Building APMrover2 VRBRAIN binaries" + ddir=$binaries/Rover/$hdate/VRX + checkout $tag || { + checkout "master" + popd + return + } + skip_build $tag $ddir || { + make vrbrain-clean && + make vrbrain || { + echo "Failed build of APMrover2 VRBRAIN $tag" + error_count=$((error_count+1)) + checkout APMrover2 "latest" "" + popd + return + } + copyit APMrover2-vrbrain-v45.vrx $ddir $tag && + copyit APMrover2-vrbrain-v45.bin $ddir $tag && + copyit APMrover2-vrbrain-v45.hex $ddir $tag && + copyit APMrover2-vrbrain-v51.vrx $ddir $tag && + copyit APMrover2-vrbrain-v51.bin $ddir $tag && + copyit APMrover2-vrbrain-v51.hex $ddir $tag && + copyit APMrover2-vrbrain-v51P.vrx $ddir $tag && + copyit APMrover2-vrbrain-v51P.bin $ddir $tag && + copyit APMrover2-vrbrain-v51P.hex $ddir $tag && + copyit APMrover2-vrbrain-v51Pro.vrx $ddir $tag && + copyit APMrover2-vrbrain-v51Pro.bin $ddir $tag && + copyit APMrover2-vrbrain-v51Pro.hex $ddir $tag && + copyit APMrover2-vrbrain-v51ProP.vrx $ddir $tag && + copyit APMrover2-vrbrain-v51ProP.bin $ddir $tag && + copyit APMrover2-vrbrain-v51ProP.hex $ddir $tag && + copyit APMrover2-vrubrain-v51.vrx $ddir $tag && + copyit APMrover2-vrubrain-v51.bin $ddir $tag && + copyit APMrover2-vrubrain-v51.hex $ddir $tag && + copyit APMrover2-vrubrain-v51P.vrx $ddir $tag && + copyit APMrover2-vrubrain-v51P.bin $ddir $tag && + copyit APMrover2-vrubrain-v51P.hex $ddir $tag + } + } + checkout "master" + popd +} + +for build in for_merge for_merge-3.2 tone_alarm tone_alarm-3.2; do + build_arduplane $build + build_arducopter $build + build_rover $build +done + +rm -rf $TMPDIR + +exit $error_count From 412de3cf76396b392d47d500e3f81d475a2152cf Mon Sep 17 00:00:00 2001 From: Emile Castelnuovo Date: Mon, 10 Nov 2014 17:08:56 +0100 Subject: [PATCH 191/254] AP_Compass: VRBRAIN add #if !defined() for VRBRAIN 4.5 boards. --- libraries/AP_Compass/AP_Compass_VRBRAIN.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp index 7f05388a668..1abe787a141 100644 --- a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp +++ b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp @@ -111,9 +111,10 @@ bool AP_Compass_VRBRAIN::read(void) // a noop on most boards _sum[i].rotate(MAG_BOARD_ORIENTATION); +#if !defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) // override any user setting of COMPASS_EXTERNAL - //_external.set(_is_external[0]); - + _external.set(_is_external[0]); +#endif if (_is_external[i]) { // add user selectable orientation _sum[i].rotate((enum Rotation)_orientation.get()); From d5108195da350fc42d585ae1a1b64cad474f7a82 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Dec 2014 14:35:36 +1100 Subject: [PATCH 192/254] HAL_PX4: FRAM does not support fsync the fsync just wastes time reopening /fs/mtd --- libraries/AP_HAL_PX4/Storage.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp index b895b39fd76..7ac62108e67 100644 --- a/libraries/AP_HAL_PX4/Storage.cpp +++ b/libraries/AP_HAL_PX4/Storage.cpp @@ -261,13 +261,6 @@ void PX4Storage::_timer_tick(void) _fd = -1; perf_count(_perf_errors); } - if (_dirty_mask == 0) { - if (fsync(_fd) != 0) { - close(_fd); - _fd = -1; - perf_count(_perf_errors); - } - } } perf_end(_perf_storage); } From 60ded486c2ccefd6d97f5d109fdea98ee0a6a826 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Dec 2014 13:29:50 +1100 Subject: [PATCH 193/254] DataFlash: don't write out parameters if log open fails --- libraries/DataFlash/LogFile.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index a0abbfc1fa1..1e764ca47b2 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -565,7 +565,10 @@ uint16_t DataFlash_Class::StartNewLog(void) { uint16_t ret; ret = start_new_log(); - + if (ret == 0xFFFF) { + // don't write out parameters if we fail to open the log + return ret; + } // write log formats so the log is self-describing for (uint8_t i=0; i<_num_types; i++) { Log_Write_Format(&_structures[i]); From 0c371f1b98031959850aee2e6b642f8b2a86b1d6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Dec 2014 13:31:27 +1100 Subject: [PATCH 194/254] DataFlash: don't try and open logfile on failure more than once this prevents a corrupted microSD card from causing a continuous attempt to open a log file while in flight, which can cause large scheduler delays Pair-Programmed-With: Grant Morphett --- libraries/DataFlash/DataFlash_File.cpp | 37 ++++++++++++++++++++------ libraries/DataFlash/DataFlash_File.h | 1 + 2 files changed, 30 insertions(+), 8 deletions(-) diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 48c659e1b7d..6ac3f1c347f 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -39,6 +39,7 @@ DataFlash_File::DataFlash_File(const char *log_directory) : _read_offset(0), _write_offset(0), _initialised(false), + _open_error(false), _log_directory(log_directory), _writebuf(NULL), _writebuf_size(16*1024), @@ -128,7 +129,7 @@ void DataFlash_File::Init(const struct LogStructure *structure, uint8_t num_type // return true for CardInserted() if we successfully initialised bool DataFlash_File::CardInserted(void) { - return _initialised; + return _initialised && !_open_error; } @@ -195,7 +196,7 @@ void DataFlash_File::EraseAll() /* Write a block of data at current offset */ void DataFlash_File::WriteBlock(const void *pBuffer, uint16_t size) { - if (_write_fd == -1 || !_initialised || !_writes_enabled) { + if (_write_fd == -1 || !_initialised || _open_error || !_writes_enabled) { return; } uint16_t _head; @@ -232,7 +233,7 @@ void DataFlash_File::WriteBlock(const void *pBuffer, uint16_t size) */ void DataFlash_File::ReadBlock(void *pkt, uint16_t size) { - if (_read_fd == -1 || !_initialised) { + if (_read_fd == -1 || !_initialised || _open_error) { return; } @@ -311,7 +312,7 @@ void DataFlash_File::get_log_boundaries(uint16_t log_num, uint16_t & start_page, */ int16_t DataFlash_File::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) { - if (!_initialised) { + if (!_initialised || _open_error) { return -1; } if (_read_fd != -1 && log_num != _read_fd_log_num) { @@ -325,10 +326,17 @@ int16_t DataFlash_File::get_log_data(uint16_t log_num, uint16_t page, uint32_t o } stop_logging(); _read_fd = ::open(fname, O_RDONLY); - free(fname); if (_read_fd == -1) { + _open_error = true; + int saved_errno = errno; + ::printf("Log read open fail for %s - %s\n", + fname, strerror(saved_errno)); + hal.console->printf("Log read open fail for %s - %s\n", + fname, strerror(saved_errno)); + free(fname); return -1; } + free(fname); _read_offset = 0; _read_fd_log_num = log_num; } @@ -408,6 +416,12 @@ uint16_t DataFlash_File::start_new_log(void) { stop_logging(); + if (_open_error) { + // we have previously failed to open a file - don't try again + // to prevent us trying to open files while in flight + return 0xFFFF; + } + if (_read_fd != -1) { ::close(_read_fd); _read_fd = -1; @@ -423,11 +437,18 @@ uint16_t DataFlash_File::start_new_log(void) } char *fname = _log_file_name(log_num); _write_fd = ::open(fname, O_WRONLY|O_CREAT|O_TRUNC, 0666); - free(fname); if (_write_fd == -1) { _initialised = false; + _open_error = true; + int saved_errno = errno; + ::printf("Log open fail for %s - %s\n", + fname, strerror(saved_errno)); + hal.console->printf("Log open fail for %s - %s\n", + fname, strerror(saved_errno)); + free(fname); return 0xFFFF; } + free(fname); _write_offset = 0; _writebuf_head = 0; _writebuf_tail = 0; @@ -452,7 +473,7 @@ void DataFlash_File::LogReadProcess(uint16_t log_num, AP_HAL::BetterStream *port) { uint8_t log_step = 0; - if (!_initialised) { + if (!_initialised || _open_error) { return; } if (_read_fd != -1) { @@ -584,7 +605,7 @@ void DataFlash_File::ListAvailableLogs(AP_HAL::BetterStream *port) void DataFlash_File::_io_timer(void) { uint16_t _tail; - if (_write_fd == -1 || !_initialised) { + if (_write_fd == -1 || !_initialised || _open_error) { return; } diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 1bd96571325..ef472e15648 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -58,6 +58,7 @@ class DataFlash_File : public DataFlash_Class uint32_t _read_offset; uint32_t _write_offset; volatile bool _initialised; + volatile bool _open_error; const char *_log_directory; /* From 506c7661a44ef5d5bfdb5f84ed4692f04e219558 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 7 Oct 2014 07:01:15 -0700 Subject: [PATCH 195/254] Copter: add throttle_zero state --- ArduCopter/ArduCopter.pde | 1 + ArduCopter/radio.pde | 17 ++++++++++++++--- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e977c955fac..dc62a5802e9 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -393,6 +393,7 @@ static union { uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes uint8_t land_complete_maybe : 1; // 18 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 19 // true if the throttle stick is at zero, debounced }; uint32_t value; } ap; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 0a274e1a834..35cc823002c 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -95,11 +95,15 @@ void output_min() motors.output_min(); } +#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 static void read_radio() { - static uint32_t last_update = 0; + static uint32_t last_update_ms = 0; + static uint32_t last_nonzero_throttle_ms = 0; + uint32_t tnow_ms = millis(); + if (hal.rcin->new_input()) { - last_update = millis(); + last_update_ms = tnow_ms; ap.new_radio_frame = true; uint16_t periods[8]; hal.rcin->read(periods,8); @@ -114,6 +118,13 @@ static void read_radio() g.rc_7.set_pwm(periods[6]); g.rc_8.set_pwm(periods[7]); + if (g.rc_3.control_in != 0) { + last_nonzero_throttle_ms = tnow_ms; + ap.throttle_zero = false; + } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { + ap.throttle_zero = true; + } + // read channels 9 ~ 14 for (uint8_t i=8; i= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) && (g.failsafe_throttle && motors.armed() && !failsafe.radio)) { From 8d63a657935b6c2d060cbacad1690693f732f02c Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 7 Oct 2014 08:29:24 -0700 Subject: [PATCH 196/254] Copter: Change all zero throttle checks that should be conservative to use ap.throttle_zero --- ArduCopter/commands_logic.pde | 2 +- ArduCopter/control_land.pde | 4 ++-- ArduCopter/control_rtl.pde | 2 +- ArduCopter/crash_check.pde | 2 +- ArduCopter/events.pde | 6 +++--- ArduCopter/fence.pde | 2 +- ArduCopter/system.pde | 6 +++--- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 384a7a88be7..6418b8f467e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -265,7 +265,7 @@ static void exit_mission() }else{ #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum - if (g.rc_3.control_in == 0 || failsafe.radio) { + if (ap.throttle_zero || failsafe.radio) { init_disarm_motors(); } #else diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index ec37b3f0d61..62dc5576908 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -61,7 +61,7 @@ static void land_gps_run() #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else @@ -131,7 +131,7 @@ static void land_nogps_run() attitude_control.set_throttle_out(0, false); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 28ce1387851..34476df681c 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -334,7 +334,7 @@ static void rtl_land_run() #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else diff --git a/ArduCopter/crash_check.pde b/ArduCopter/crash_check.pde index 26701c90ae9..11ad911e137 100644 --- a/ArduCopter/crash_check.pde +++ b/ArduCopter/crash_check.pde @@ -25,7 +25,7 @@ void crash_check() #endif // return immediately if motors are not armed or pilot's throttle is above zero - if (!motors.armed() || (g.rc_3.control_in != 0 && !failsafe.radio)) { + if (!motors.armed() || (!ap.throttle_zero && !failsafe.radio)) { inverted_count = 0; return; } diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index ade72ff7713..aec06ff8daa 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -16,7 +16,7 @@ static void failsafe_radio_on_event() case STABILIZE: case ACRO: // if throttle is zero OR vehicle is landed disarm motors - if (g.rc_3.control_in == 0 || ap.land_complete) { + if (ap.throttle_zero || ap.land_complete) { init_disarm_motors(); // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately @@ -115,7 +115,7 @@ static void failsafe_battery_event(void) case STABILIZE: case ACRO: // if throttle is zero OR vehicle is landed disarm motors - if (g.rc_3.control_in == 0 || ap.land_complete) { + if (ap.throttle_zero || ap.land_complete) { init_disarm_motors(); }else{ // set mode to RTL or LAND @@ -274,7 +274,7 @@ static void failsafe_gcs_check() case ACRO: case SPORT: // if throttle is zero disarm motors - if (g.rc_3.control_in == 0) { + if (ap.throttle_zero) { init_disarm_motors(); }else if(home_distance > wp_nav.get_wp_radius()) { if (!set_mode(RTL)) { diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde index bfe3a8d979d..be2ba06e7b2 100644 --- a/ArduCopter/fence.pde +++ b/ArduCopter/fence.pde @@ -30,7 +30,7 @@ void fence_check() // disarm immediately if we think we are on the ground // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down - if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ + if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ init_disarm_motors(); }else{ // if we are within 100m of the fence, RTL diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 4e34d8cb4ef..5aa94634943 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -356,7 +356,7 @@ static void update_auto_armed() return; } // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false - if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio) { + if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio) { set_auto_armed(false); } }else{ @@ -364,12 +364,12 @@ static void update_auto_armed() #if FRAME_CONFIG == HELI_FRAME // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true - if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) { + if(motors.armed() && !ap.throttle_zero && motors.motor_runup_complete()) { set_auto_armed(true); } #else // if motors are armed and throttle is above zero auto_armed should be true - if(motors.armed() && g.rc_3.control_in != 0) { + if(motors.armed() && !ap.throttle_zero) { set_auto_armed(true); } #endif // HELI_FRAME From 9ced648479d348a0a3fee653f11abcb55b29314f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 21:52:16 +0900 Subject: [PATCH 197/254] Copter: throttle zero debounce to separate function Also initialise throttle_zero to true on startup Treat throttle less than zero as zero --- ArduCopter/radio.pde | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 35cc823002c..b1e78803ea2 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -38,6 +38,9 @@ static void init_rc_in() // set default dead zones default_dead_zones(); + + // initialise throttle_zero flag + ap.throttle_zero = true; } // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration @@ -95,11 +98,9 @@ void output_min() motors.output_min(); } -#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 static void read_radio() { static uint32_t last_update_ms = 0; - static uint32_t last_nonzero_throttle_ms = 0; uint32_t tnow_ms = millis(); if (hal.rcin->new_input()) { @@ -111,6 +112,7 @@ static void read_radio() g.rc_2.set_pwm(periods[rcmap.pitch()-1]); set_throttle_and_failsafe(periods[rcmap.throttle()-1]); + set_throttle_zero_flag(g.rc_3.control_in); g.rc_4.set_pwm(periods[rcmap.yaw()-1]); g.rc_5.set_pwm(periods[4]); @@ -118,13 +120,6 @@ static void read_radio() g.rc_7.set_pwm(periods[6]); g.rc_8.set_pwm(periods[7]); - if (g.rc_3.control_in != 0) { - last_nonzero_throttle_ms = tnow_ms; - ap.throttle_zero = false; - } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { - ap.throttle_zero = true; - } - // read channels 9 ~ 14 for (uint8_t i=8; i 0) { + last_nonzero_throttle_ms = tnow_ms; + ap.throttle_zero = false; + } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) { + ap.throttle_zero = true; + } +} + static void trim_radio() { for (uint8_t i = 0; i < 30; i++) { From f64f155c3a88dc93344a56ae9e5885e1fa68b69d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:08:26 -0700 Subject: [PATCH 198/254] Copter: add land_complete to fence disarm check --- ArduCopter/fence.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde index be2ba06e7b2..e1eacf54b5b 100644 --- a/ArduCopter/fence.pde +++ b/ArduCopter/fence.pde @@ -30,7 +30,7 @@ void fence_check() // disarm immediately if we think we are on the ground // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down - if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ + if(ap.land_complete || manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ init_disarm_motors(); }else{ // if we are within 100m of the fence, RTL From 9fc62b5db2c710b73428176e9f163396e45396ea Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:11:24 -0700 Subject: [PATCH 199/254] Copter: use ap.throttle_zero instead of rc_3.control_in in auto_disarm_check --- ArduCopter/motors.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 6147efa3b24..45997d3078c 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -103,7 +103,7 @@ static void arm_motors_check() static void auto_disarm_check() { // exit immediately if we are already disarmed or throttle is not zero - if (!motors.armed() || g.rc_3.control_in > 0) { + if (!motors.armed() || !ap.throttle_zero) { auto_disarming_counter = 0; return; } From 7ea5e69f9cb5e69e36923d51c761beea3246df2b Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:15:00 -0700 Subject: [PATCH 200/254] Copter: auto-disarm if land complete regardless of mode --- ArduCopter/motors.pde | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 45997d3078c..8647cc70e2b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -109,9 +109,7 @@ static void auto_disarm_check() } // allow auto disarm in manual flight modes or Loiter/AltHold if we're landed - if (manual_flight_mode(control_mode) || (ap.land_complete && (control_mode == ALT_HOLD || control_mode == LOITER || control_mode == OF_LOITER || - control_mode == DRIFT || control_mode == SPORT || control_mode == AUTOTUNE || - control_mode == POSHOLD))) { + if (manual_flight_mode(control_mode) || ap.land_complete) { auto_disarming_counter++; if(auto_disarming_counter >= AUTO_DISARMING_DELAY) { From e05f8d308709ba43c2893dcdcda129a5f8bc1ca7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:26:43 -0700 Subject: [PATCH 201/254] Copter: add mode_allows_arming function --- ArduCopter/flight_mode.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index a0f4fee88f0..e12b4c9f0b4 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -267,6 +267,13 @@ static bool manual_flight_mode(uint8_t mode) { return false; } +static bool mode_allows_arming(uint8_t mode) { + if (manual_flight_mode(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == AUTOTUNE || mode == GUIDED) { + return true; + } + return false; +} + // // print_flight_mode - prints flight mode to serial port. // From c65cb45c079e2c099bb011e6dc552b914e3a083c Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 10 Oct 2014 02:29:10 -0700 Subject: [PATCH 202/254] Copter: move all arm check logic into arm_checks --- ArduCopter/GCS_Mavlink.pde | 4 +-- ArduCopter/motors.pde | 52 ++++++++++++++++++-------------------- 2 files changed, 27 insertions(+), 29 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index c7fb2c42914..e6e611824af 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1158,7 +1158,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1.0f) { // run pre_arm_checks and arm_checks and display failures pre_arm_checks(true); - if(ap.pre_arm_check && arm_checks(true)) { + if(ap.pre_arm_check && arm_checks(true, true)) { if (init_arm_motors()) { result = MAV_RESULT_ACCEPTED; } else { @@ -1169,7 +1169,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false result = MAV_RESULT_UNSUPPORTED; } - } else if (packet.param1 == 0.0f) { + } else if (packet.param1 == 0.0f && (manual_flight_mode(control_mode) || ap.land_complete)) { init_disarm_motors(); result = MAV_RESULT_ACCEPTED; } else { diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 8647cc70e2b..c1f893a9fd9 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -12,7 +12,6 @@ static uint8_t auto_disarming_counter; static void arm_motors_check() { static int16_t arming_counter; - bool allow_arming = false; // ensure throttle is down if (g.rc_3.control_in > 0) { @@ -20,30 +19,6 @@ static void arm_motors_check() return; } - // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT - if (manual_flight_mode(control_mode)) { - allow_arming = true; - } - - // allow arming/disarming in Loiter and AltHold if landed - if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) { - allow_arming = true; - } - - // kick out other flight modes - if (!allow_arming) { - arming_counter = 0; - return; - } - - #if FRAME_CONFIG == HELI_FRAME - // heli specific arming check - if (!motors.allow_arming()){ - arming_counter = 0; - return; - } - #endif // HELI_FRAME - int16_t tmp = g.rc_4.control_in; // full right @@ -58,7 +33,7 @@ static void arm_motors_check() if (arming_counter == ARM_DELAY && !motors.armed()) { // run pre-arm-checks and display failures pre_arm_checks(true); - if(ap.pre_arm_check && arm_checks(true)) { + if(ap.pre_arm_check && arm_checks(true,false)) { if (!init_arm_motors()) { // reset arming counter if arming fail arming_counter = 0; @@ -80,6 +55,10 @@ static void arm_motors_check() // full left }else if (tmp < -4000) { + if (!manual_flight_mode(control_mode) && !ap.land_complete) { + arming_counter = 0; + return; + } // increase the counter to a maximum of 1 beyond the disarm delay if( arming_counter <= DISARM_DELAY ) { @@ -549,8 +528,27 @@ static bool pre_arm_gps_checks(bool display_failure) // arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm -static bool arm_checks(bool display_failure) +static bool arm_checks(bool display_failure, bool request_from_gcs) { + // always check if the current mode allows arming + if (!mode_allows_arming(control_mode) || (!request_from_gcs && control_mode == GUIDED)) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable")); + } + return false; + } + + // always check if rotor is spinning on heli + #if FRAME_CONFIG == HELI_FRAME + // heli specific arming check + if (!motors.allow_arming()){ + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor not spinning")); + } + return false; + } + #endif // HELI_FRAME + // succeed if arming checks are disabled if (g.arming_check == ARMING_CHECK_NONE) { return true; From 6327f4adf187b89d8fd2324e5fe15b8302261249 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 11 Oct 2014 16:05:32 +0900 Subject: [PATCH 203/254] Copter: allow arming in GUIDED only from GCS Also changed mode_allows_arming function to accept arming_from_gcs param Also remove AUTOTUNE from arming list --- ArduCopter/flight_mode.pde | 6 ++++-- ArduCopter/motors.pde | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index e12b4c9f0b4..a87d1b714d1 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -267,8 +267,10 @@ static bool manual_flight_mode(uint8_t mode) { return false; } -static bool mode_allows_arming(uint8_t mode) { - if (manual_flight_mode(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == AUTOTUNE || mode == GUIDED) { +// mode_allows_arming - returns true if vehicle can be armed in the specified mode +// arming_from_gcs should be set to true if the arming request comes from the ground station +static bool mode_allows_arming(uint8_t mode, bool arming_from_gcs) { + if (manual_flight_mode(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) { return true; } return false; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index c1f893a9fd9..22715897287 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -528,10 +528,10 @@ static bool pre_arm_gps_checks(bool display_failure) // arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm -static bool arm_checks(bool display_failure, bool request_from_gcs) +static bool arm_checks(bool display_failure, bool arming_from_gcs) { // always check if the current mode allows arming - if (!mode_allows_arming(control_mode) || (!request_from_gcs && control_mode == GUIDED)) { + if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable")); } From 77bbd2532b6ce98ff0b185d763a70fc3c1864c93 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 13 Nov 2014 16:56:38 -0800 Subject: [PATCH 204/254] AC_PosControl: add force_descend option to set_alt_target_from_climb_rate --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++-- libraries/AC_AttitudeControl/AC_PosControl.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 37714efaaca..0cd3a5d6d19 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -137,11 +137,11 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) /// should be called continuously (with dt set to be the expected time between calls) /// actual position target will be moved no faster than the speed_down and speed_up /// target will also be stopped if the motors hit their limits or leash length is exceeded -void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt) +void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) { // adjust desired alt if motors have not hit their limits // To-Do: add check of _limit.pos_up and _limit.pos_down? - if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) { + if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) { _pos_target.z += climb_rate_cms * dt; } } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 52ffaa02e72..08bc77440b4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -109,7 +109,7 @@ class AC_PosControl /// should be called continuously (with dt set to be the expected time between calls) /// actual position target will be moved no faster than the speed_down and speed_up /// target will also be stopped if the motors hit their limits or leash length is exceeded - void set_alt_target_from_climb_rate(float climb_rate_cms, float dt); + void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend = true); /// set_alt_target_to_current_alt - set altitude target to current altitude void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); } From f3e9e9cc6fe8290781a2de6f5e2a5ca00edfdd7d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 13 Nov 2014 16:57:25 -0800 Subject: [PATCH 205/254] Copter: use force_descend option on auto landings --- ArduCopter/control_auto.pde | 2 +- ArduCopter/control_land.pde | 4 ++-- ArduCopter/control_rtl.pde | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 6648f46bbce..6f28aa72298 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -315,7 +315,7 @@ static void auto_land_run() wp_nav.update_loiter(); // call z-axis position controller - pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); + pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt, true); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 62dc5576908..68c22e5068f 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -112,7 +112,7 @@ static void land_gps_run() } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); } @@ -170,7 +170,7 @@ static void land_nogps_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 34476df681c..7fc52232328 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -377,7 +377,7 @@ static void rtl_land_run() // call z-axis position controller float cmb_rate = get_throttle_land(); - pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt); + pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot From 59350821a3d3dea7ed141bda0a1ff659855b490d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 13 Nov 2014 18:38:55 -0800 Subject: [PATCH 206/254] AC_PosControl: fix to default force_descend param --- libraries/AC_AttitudeControl/AC_PosControl.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 08bc77440b4..0347c51511e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -109,7 +109,8 @@ class AC_PosControl /// should be called continuously (with dt set to be the expected time between calls) /// actual position target will be moved no faster than the speed_down and speed_up /// target will also be stopped if the motors hit their limits or leash length is exceeded - void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend = true); + /// set force_descend to true during landing to allow target to move low enough to slow the motors + void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend = false); /// set_alt_target_to_current_alt - set altitude target to current altitude void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); } From 44c5fdffdf44f9da045485e922a5cd2e0c73f3f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Dec 2014 22:23:00 +0900 Subject: [PATCH 207/254] Notify: add pre_arm_gps_check flag RGB LED will remain flashing blue when vehicle is disarmed and this check has failed (i.e. false). --- libraries/AP_Notify/AP_Notify.h | 1 + libraries/AP_Notify/ToshibaLED.cpp | 24 +++++++++++++----------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 8e4924ff2dd..594094e6fd4 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -38,6 +38,7 @@ class AP_Notify uint16_t baro_glitching : 1; // 1 if baro altitude is not good uint16_t armed : 1; // 0 = disarmed, 1 = armed uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed + uint16_t pre_arm_gps_check : 1; // 0 = failing pre-arm GPS checks, 1 = passed uint16_t save_trim : 1; // 1 if gathering trim data uint16_t esc_calibration : 1; // 1 if calibrating escs uint16_t failsafe_radio : 1; // 1 if radio failsafe diff --git a/libraries/AP_Notify/ToshibaLED.cpp b/libraries/AP_Notify/ToshibaLED.cpp index 1460c0538ea..72341c1d863 100644 --- a/libraries/AP_Notify/ToshibaLED.cpp +++ b/libraries/AP_Notify/ToshibaLED.cpp @@ -223,32 +223,34 @@ void ToshibaLED::update_colours(void) break; } }else{ - // flashing green if disarmed with GPS 3d lock - // flashing blue if disarmed with no gps lock + // fast flashing green if disarmed with GPS 3D lock and DGPS + // slow flashing green if disarmed with GPS 3d lock (and no DGPS) + // flashing blue if disarmed with no gps lock or gps pre_arm checks have failed + bool fast_green = AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check; switch(step) { case 0: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = brightness; } break; case 1: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = TOSHIBA_LED_OFF; } break; case 2: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = brightness; } break; case 3: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = TOSHIBA_LED_OFF; } break; case 4: _red_des = TOSHIBA_LED_OFF; - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) { // flashing green if disarmed with GPS 3d lock _blue_des = TOSHIBA_LED_OFF; _green_des = brightness; @@ -259,24 +261,24 @@ void ToshibaLED::update_colours(void) } break; case 5: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = TOSHIBA_LED_OFF; } break; case 6: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = brightness; } break; case 7: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = TOSHIBA_LED_OFF; } break; case 8: - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + if (fast_green) { _green_des = brightness; } break; From 6b236eb7eacef34908bb0356c1baeebdec580dbb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Dec 2014 22:25:53 +0900 Subject: [PATCH 208/254] Copter: set pre_arm_gps_check flag --- ArduCopter/motors.pde | 60 +++++++++++++++++++++++++++++++------------ 1 file changed, 44 insertions(+), 16 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 22715897287..92d2d3402b8 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -217,6 +217,7 @@ static void pre_arm_checks(bool display_failure) { // exit immediately if we've already successfully performed the pre-arm check if (ap.pre_arm_check) { + pre_arm_gps_checks(display_failure); return; } @@ -313,18 +314,16 @@ static void pre_arm_checks(bool display_failure) } // check GPS - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) { - // check gps is ok if required - note this same check is repeated again in arm_checks - if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) { - return; - } + if (!pre_arm_gps_checks(display_failure)) { + return; + } -#if AC_FENCE == ENABLED - // check fence is initialised - if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) { - return; + // check fence is initialised + if(!fence.pre_arm_check()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check fence")); } -#endif + return; } // check INS @@ -488,13 +487,39 @@ static void pre_arm_rc_checks() // performs pre_arm gps related checks and returns true if passed static bool pre_arm_gps_checks(bool display_failure) { - float speed_cms = inertial_nav.get_velocity().length(); // speed according to inertial nav in cm/s + // return true immediately if gps check is disabled + if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // check if flight mode requires GPS + bool gps_required = mode_requires_GPS(control_mode); + + // if GPS failsafe will triggers even in stabilize mode we need GPS before arming + if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) { + gps_required = true; + } + +#if AC_FENCE == ENABLED + // if circular fence is enabled we need GPS + if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { + gps_required = true; + } +#endif + + // return true if GPS is not required + if (!gps_required) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } // check GPS is not glitching if (gps_glitch.glitching()) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch")); } + AP_Notify::flags.pre_arm_gps_check = false; return false; } @@ -503,14 +528,17 @@ static bool pre_arm_gps_checks(bool display_failure) if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix")); } + AP_Notify::flags.pre_arm_gps_check = false; return false; } // check speed is below 50cm/s + float speed_cms = inertial_nav.get_velocity().length(); // speed according to inertial nav in cm/s if (speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad Velocity")); } + AP_Notify::flags.pre_arm_gps_check = false; return false; } @@ -519,10 +547,12 @@ static bool pre_arm_gps_checks(bool display_failure) if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP")); } + AP_Notify::flags.pre_arm_gps_check = false; return false; } // if we got here all must be ok + AP_Notify::flags.pre_arm_gps_check = true; return true; } @@ -564,11 +594,9 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs) } } - // check gps is ok if required - note this same check is also done in pre-arm checks - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) { - if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) { - return false; - } + // check gps + if (!pre_arm_gps_checks(display_failure)) { + return false; } // check parameters From 152a3a2828f47a026a9db05d3ab5e7859188548a Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 6 Jan 2015 00:44:52 -0500 Subject: [PATCH 209/254] ArduCopter: Bug fix, int8t should be uint16t. --- ArduCopter/control_poshold.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 562f5e72738..ab201a84e39 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -77,7 +77,7 @@ static struct { // loiter related variables int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT + int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) @@ -85,8 +85,8 @@ static struct { Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller int16_t wind_comp_roll; // roll angle to compensate for wind int16_t wind_comp_pitch; // pitch angle to compensate for wind - int8_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged - int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz + uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged + int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz // final output int16_t roll; // final roll angle sent to attitude controller From 1266a31631406cfea3dff2e3970c06682af9b053 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 6 Jan 2015 15:55:32 +0900 Subject: [PATCH 210/254] Rally: reduce distance limit to 300m for copter This reduces the chance that a forgotten rally point will cause the vehicle to RTL to a distant location because instead it will RTL to home. --- libraries/AP_Rally/AP_Rally.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index fee98c28b99..bdf1e7d6e8e 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -13,7 +13,7 @@ StorageAccess AP_Rally::_storage(StorageManager::StorageRally); // ArduCopter/defines.h sets this, and this definition will be moved into ArduPlane/defines.h when that is patched to use the lib #ifdef APM_BUILD_DIRECTORY #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - #define RALLY_LIMIT_KM_DEFAULT 2.0 + #define RALLY_LIMIT_KM_DEFAULT 0.3 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define RALLY_LIMIT_KM_DEFAULT 5.0 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) From 4033f11a3c5c789ee898b344fa5090dee098f566 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 6 Jan 2015 16:38:34 +0900 Subject: [PATCH 211/254] Copter: report NAV_CONTROLLER_OUTPUT in RTL, Guided This allows the GCS to display to the user where the vehicle is flying to in RTL and Guided flight modes --- ArduCopter/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 92f702e999d..1d700d65762 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -38,7 +38,7 @@ static void calc_wp_distance() // get target from loiter or wpinav controller if (control_mode == LOITER || control_mode == CIRCLE) { wp_distance = wp_nav.get_loiter_distance_to_target(); - }else if (control_mode == AUTO) { + }else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) { wp_distance = wp_nav.get_wp_distance_to_destination(); }else{ wp_distance = 0; @@ -51,7 +51,7 @@ static void calc_wp_bearing() // get target from loiter or wpinav controller if (control_mode == LOITER || control_mode == CIRCLE) { wp_bearing = wp_nav.get_loiter_bearing_to_target(); - } else if (control_mode == AUTO) { + } else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) { wp_bearing = wp_nav.get_wp_bearing_to_destination(); } else { wp_bearing = 0; From 105e2e19ac5b430a2ce0905b3f3ae416ad1ea03e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Dec 2014 21:52:04 +0900 Subject: [PATCH 212/254] Copter: skip pre-arm checks when already armed --- ArduCopter/motors.pde | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 92d2d3402b8..3f55f25be12 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -215,7 +215,13 @@ static bool init_arm_motors() // perform pre-arm checks and set ap.pre_arm_check flag static void pre_arm_checks(bool display_failure) { + // exit immediately if already armed + if (motors.armed()) { + return; + } + // exit immediately if we've already successfully performed the pre-arm check + // run gps checks because results may change and affect LED colour if (ap.pre_arm_check) { pre_arm_gps_checks(display_failure); return; From aa1b0da25499662a118e22598160307ed789503a Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sun, 2 Nov 2014 15:20:13 -0800 Subject: [PATCH 213/254] Copter: run_nav_updates at 100hz on pixhawk --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index dc62a5802e9..8f89997cb16 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -801,7 +801,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { arm_motors_check, 40, 1 }, { auto_trim, 40, 14 }, { update_altitude, 40, 100 }, - { run_nav_updates, 40, 80 }, + { run_nav_updates, 4, 80 }, { update_thr_cruise, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 8, 42 }, From f5fb21b1d7e29c6ca136169b073fb215ca97f835 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 5 Dec 2014 13:59:29 +0900 Subject: [PATCH 214/254] Copter: run_nav_updates at 50hz on Pixhawk, 25hz on APM2 Based on work by Jon Challinger (see earlier commit) --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 8f89997cb16..f03d2a5bbf0 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -801,7 +801,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { arm_motors_check, 40, 1 }, { auto_trim, 40, 14 }, { update_altitude, 40, 100 }, - { run_nav_updates, 4, 80 }, + { run_nav_updates, 8, 80 }, { update_thr_cruise, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 8, 42 }, @@ -869,7 +869,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { arm_motors_check, 10, 10 }, { auto_trim, 10, 140 }, { update_altitude, 10, 1000 }, - { run_nav_updates, 10, 800 }, + { run_nav_updates, 4, 800 }, { update_thr_cruise, 1, 50 }, { three_hz_loop, 33, 90 }, { compass_accumulate, 2, 420 }, From 0ad7a39db25e2d676192c9de9944c05fbe095317 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:18:44 +1100 Subject: [PATCH 215/254] Copter: import new travis config --- .travis.yml | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 8025bc6efad..33b8576a7bf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,7 +4,12 @@ before_install: - cd .. && ./ardupilot/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile script: - - cd ./ardupilot/ArduCopter && make configure && make && make px4-v2 - - cd ../ArduPlane && make configure && make && make px4-v2 - - cd ../APMrover2 && make configure && make && make px4-v2 - - cd ../ArduCopter && make configure && make && make vrubrain-v51 + - cd ./ardupilot && Tools/scripts/build_all_travis.sh + +notifications: + webhooks: + urls: + - https://webhooks.gitter.im/e/e5e0b55e353e53945b4b + on_success: change # options: [always|never|change] default: always + on_failure: always # options: [always|never|change] default: always + on_start: false # default: false From 827e60f948edcaf4ee3b4f6352eeab51180b840e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:19:11 +1100 Subject: [PATCH 216/254] AP_InertialSensor: roll up to latest AP_InertialSensor from master --- .../AP_InertialSensor/AP_InertialSensor.cpp | 460 +++++++++++++++++- .../AP_InertialSensor/AP_InertialSensor.h | 197 +++++--- .../AP_InertialSensor_Flymaple.cpp | 206 +++----- .../AP_InertialSensor_Flymaple.h | 56 +-- .../AP_InertialSensor_HIL.cpp | 136 +----- .../AP_InertialSensor/AP_InertialSensor_HIL.h | 36 +- .../AP_InertialSensor_L3G4200D.cpp | 134 ++--- .../AP_InertialSensor_L3G4200D.h | 41 +- .../AP_InertialSensor_MPU6000.cpp | 282 +++++------ .../AP_InertialSensor_MPU6000.h | 82 ++-- .../AP_InertialSensor_MPU9150.cpp | 187 +++---- .../AP_InertialSensor_MPU9150.h | 34 +- .../AP_InertialSensor_MPU9250.cpp | 427 ++++++---------- .../AP_InertialSensor_MPU9250.h | 90 ++-- .../AP_InertialSensor_Oilpan.cpp | 146 +++--- .../AP_InertialSensor_Oilpan.h | 41 +- .../AP_InertialSensor_PX4.cpp | 229 +++------ .../AP_InertialSensor/AP_InertialSensor_PX4.h | 46 +- .../examples/INS_generic/INS_generic.pde | 24 +- 19 files changed, 1366 insertions(+), 1488 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 43617189d46..f53572ba7b9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -90,14 +90,126 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0), #if INS_MAX_INSTANCES > 1 + // @Param: ACC2SCAL_X + // @DisplayName: Accelerometer2 scaling of X axis + // @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC2SCAL_Y + // @DisplayName: Accelerometer2 scaling of Y axis + // @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC2SCAL_Z + // @DisplayName: Accelerometer2 scaling of Z axis + // @Description: Accelerometer2 scaling of Z axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced AP_GROUPINFO("ACC2SCAL", 5, AP_InertialSensor, _accel_scale[1], 0), + + // @Param: ACC2OFFS_X + // @DisplayName: Accelerometer2 offsets of X axis + // @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC2OFFS_Y + // @DisplayName: Accelerometer2 offsets of Y axis + // @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC2OFFS_Z + // @DisplayName: Accelerometer2 offsets of Z axis + // @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced AP_GROUPINFO("ACC2OFFS", 6, AP_InertialSensor, _accel_offset[1], 0), + + // @Param: GYR2OFFS_X + // @DisplayName: Gyro2 offsets of X axis + // @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR2OFFS_Y + // @DisplayName: Gyro2 offsets of Y axis + // @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR2OFFS_Z + // @DisplayName: Gyro2 offsets of Z axis + // @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0), #endif #if INS_MAX_INSTANCES > 2 + // @Param: ACC3SCAL_X + // @DisplayName: Accelerometer3 scaling of X axis + // @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC3SCAL_Y + // @DisplayName: Accelerometer3 scaling of Y axis + // @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC3SCAL_Z + // @DisplayName: Accelerometer3 scaling of Z axis + // @Description: Accelerometer3 scaling of Z axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced AP_GROUPINFO("ACC3SCAL", 8, AP_InertialSensor, _accel_scale[2], 0), + + // @Param: ACC3OFFS_X + // @DisplayName: Accelerometer3 offsets of X axis + // @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC3OFFS_Y + // @DisplayName: Accelerometer3 offsets of Y axis + // @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC3OFFS_Z + // @DisplayName: Accelerometer3 offsets of Z axis + // @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced AP_GROUPINFO("ACC3OFFS", 9, AP_InertialSensor, _accel_offset[2], 0), + + // @Param: GYR3OFFS_X + // @DisplayName: Gyro3 offsets of X axis + // @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR3OFFS_Y + // @DisplayName: Gyro3 offsets of Y axis + // @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR3OFFS_Z + // @DisplayName: Gyro3 offsets of Z axis + // @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0), #endif @@ -105,30 +217,154 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { }; AP_InertialSensor::AP_InertialSensor() : + _gyro_count(0), + _accel_count(0), + _backend_count(0), _accel(), _gyro(), - _board_orientation(ROTATION_NONE) + _board_orientation(ROTATION_NONE), + _hil_mode(false), + _have_3D_calibration(false) { AP_Param::setup_object_defaults(this, var_info); + for (uint8_t i=0; ipanic(PSTR("Too many gyros")); + } + return _gyro_count++; +} + +/* + register a new accel instance + */ +uint8_t AP_InertialSensor::register_accel(void) +{ + if (_accel_count == INS_MAX_INSTANCES) { + hal.scheduler->panic(PSTR("Too many accels")); + } + return _accel_count++; } void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) { - _product_id = _init_sensor(sample_rate); + // remember the sample rate + _sample_rate = sample_rate; - // check scaling + if (_gyro_count == 0 && _accel_count == 0) { + // detect available backends. Only called once + _detect_backends(); + } + + _product_id = 0; // FIX + + // initialise accel scale if need be. This is needed as we can't + // give non-zero default values for vectors in AP_Param for (uint8_t i=0; ipanic(PSTR("Too many INS backends")); + } + _backends[_backend_count] = detect(*this); + if (_backends[_backend_count] != NULL) { + _backend_count++; + } +} + + +/* + detect available backends for this board + */ +void +AP_InertialSensor::_detect_backends(void) +{ +#if HAL_INS_DEFAULT == HAL_INS_HIL + _add_backend(AP_InertialSensor_HIL::detect); +#elif HAL_INS_DEFAULT == HAL_INS_MPU6000 + _add_backend(AP_InertialSensor_MPU6000::detect); +#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN + _add_backend(AP_InertialSensor_PX4::detect); +#elif HAL_INS_DEFAULT == HAL_INS_OILPAN + _add_backend(AP_InertialSensor_Oilpan::detect); +#elif HAL_INS_DEFAULT == HAL_INS_MPU9250 + _add_backend(AP_InertialSensor_MPU9250::detect); +#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE + _add_backend(AP_InertialSensor_Flymaple::detect); +#else + #error Unrecognised HAL_INS_TYPE setting +#endif + +#if 0 // disabled due to broken hardware on some PXF capes +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + // the PXF also has a MPU6000 + _add_backend(AP_InertialSensor_MPU6000::detect); +#endif +#endif + + if (_backend_count == 0 || + _gyro_count == 0 || + _accel_count == 0) { + hal.scheduler->panic(PSTR("No INS backends available")); + } + + // set the product ID to the ID of the first backend + _product_id.set(_backends[0]->product_id()); } void @@ -138,6 +374,8 @@ AP_InertialSensor::init_accel() // save calibration _save_parameters(); + + check_3D_calibration(); } #if !defined( __AVR_ATmega1280__ ) @@ -213,10 +451,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact } uint8_t num_samples = 0; while (num_samples < 32) { - if (!wait_for_sample(1000)) { - interact->printf_P(PSTR("Failed to get INS sample\n")); - goto failed; - } + wait_for_sample(); // read samples from ins update(); // capture sample @@ -254,6 +489,8 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact } _save_parameters(); + check_3D_calibration(); + // calculate the trims as well from primary accels and pass back to caller _calculate_trim(samples[0][0], trim_roll, trim_pitch); @@ -271,18 +508,37 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact } #endif -/// calibrated - returns true if the accelerometers have been calibrated -/// @note this should not be called while flying because it reads from the eeprom which can be slow -bool AP_InertialSensor::calibrated() +/* + check if the accelerometers are calibrated in 3D. Called on startup + and any accel cal + */ +void AP_InertialSensor::check_3D_calibration() { + _have_3D_calibration = false; // check each accelerometer has offsets saved for (uint8_t i=0; iupdate(); + } + + // adjust health status if a sensor has a non-zero error count + // but another sensor doesn't. + bool have_zero_accel_error_count = false; + bool have_zero_gyro_error_count = false; + for (uint8_t i=0; imicros(); + + if (_next_sample_usec == 0 && _delta_time <= 0) { + // this is the first call to wait_for_sample() + _last_sample_usec = now - _sample_period_usec; + _next_sample_usec = now + _sample_period_usec; + goto check_sample; + } + + // see how long it is till the next sample is due + if (_next_sample_usec - now <=_sample_period_usec) { + // we're ahead on time, schedule next sample at expected period + uint32_t wait_usec = _next_sample_usec - now; + if (wait_usec > 200) { + hal.scheduler->delay_microseconds(wait_usec); + } + _next_sample_usec += _sample_period_usec; + } else if (now - _next_sample_usec < _sample_period_usec/8) { + // we've overshot, but only by a small amount, keep on + // schedule with no delay + _next_sample_usec += _sample_period_usec; + } else { + // we've overshot by a larger amount, re-zero scheduling with + // no delay + _next_sample_usec = now + _sample_period_usec; + } + +check_sample: + if (!_hil_mode) { + // we also wait for at least one backend to have a sample of both + // accel and gyro. This normally completes immediately. + bool gyro_available = false; + bool accel_available = false; + while (!gyro_available || !accel_available) { + for (uint8_t i=0; i<_backend_count; i++) { + gyro_available |= _backends[i]->gyro_sample_available(); + accel_available |= _backends[i]->accel_sample_available(); + } + if (!gyro_available || !accel_available) { + hal.scheduler->delay_microseconds(100); + } + } + } + + now = hal.scheduler->micros(); + _delta_time = (now - _last_sample_usec) * 1.0e-6f; + _last_sample_usec = now; + +#if 0 + { + static uint64_t delta_time_sum; + static uint16_t counter; + if (delta_time_sum == 0) { + delta_time_sum = _sample_period_usec; + } + delta_time_sum += _delta_time * 1.0e6f; + if (counter++ == 400) { + counter = 0; + hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n", + (unsigned long)now, + (unsigned long)delta_time_sum, + (long)(now - delta_time_sum)); + } + } +#endif + + _have_sample = true; +} + +/* + support for setting accel and gyro vectors, for use by HIL + */ +void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel) +{ + if (instance < INS_MAX_INSTANCES) { + _accel[instance] = accel; + _accel_healthy[instance] = true; + } +} + +void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro) +{ + if (instance < INS_MAX_INSTANCES) { + _gyro[instance] = gyro; + _gyro_healthy[instance] = true; + } +} + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 3eaba837557..12004e1edeb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -11,18 +11,22 @@ maximum number of INS instances available on this platform. If more than 1 then redundent sensors may be available */ -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL -#define INS_MAX_INSTANCES 3 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 #define INS_MAX_INSTANCES 3 +#define INS_MAX_BACKENDS 6 #else #define INS_MAX_INSTANCES 1 +#define INS_MAX_BACKENDS 1 #endif + #include #include #include #include "AP_InertialSensor_UserInteract.h" + +class AP_InertialSensor_Backend; + /* AP_InertialSensor is an abstraction for gyro and accel measurements * which are correctly aligned to the body axes and scaled to SI units. * @@ -32,12 +36,11 @@ */ class AP_InertialSensor { + friend class AP_InertialSensor_Backend; + public: AP_InertialSensor(); - // empty virtual destructor - virtual ~AP_InertialSensor() {} - enum Start_style { COLD_START = 0, WARM_START @@ -64,22 +67,28 @@ class AP_InertialSensor /// /// @param style The initialisation startup style. /// - virtual void init( Start_style style, - Sample_rate sample_rate); + void init( Start_style style, + Sample_rate sample_rate); /// Perform cold startup initialisation for just the accelerometers. /// /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work. /// - virtual void init_accel(); + void init_accel(); + + + /// Register a new gyro/accel driver, allocating an instance + /// number + uint8_t register_gyro(void); + uint8_t register_accel(void); #if !defined( __AVR_ATmega1280__ ) // perform accelerometer calibration including providing user instructions // and feedback - virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact, - float& trim_roll, - float& trim_pitch); + bool calibrate_accel(AP_InertialSensor_UserInteract *interact, + float& trim_roll, + float& trim_pitch); #endif /// calibrated - returns true if the accelerometers have been calibrated @@ -93,65 +102,66 @@ class AP_InertialSensor /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work /// - virtual void init_gyro(void); + void init_gyro(void); /// Fetch the current gyro values /// /// @returns vector of rotational rates in radians/sec /// const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; } - const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); } - virtual void set_gyro(uint8_t instance, const Vector3f &gyro) {} + const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); } + void set_gyro(uint8_t instance, const Vector3f &gyro); // set gyro offsets in radians/sec const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; } - const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); } + const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); } /// Fetch the current accelerometer values /// /// @returns vector of current accelerations in m/s/s /// const Vector3f &get_accel(uint8_t i) const { return _accel[i]; } - const Vector3f &get_accel(void) const { return get_accel(get_primary_accel()); } - virtual void set_accel(uint8_t instance, const Vector3f &accel) {} + const Vector3f &get_accel(void) const { return get_accel(_primary_accel); } + void set_accel(uint8_t instance, const Vector3f &accel); + + uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; } + uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; } // multi-device interface - virtual bool get_gyro_health(uint8_t instance) const { return true; } - bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } + bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; } + bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); } bool get_gyro_health_all(void) const; - virtual uint8_t get_gyro_count(void) const { return 1; }; + uint8_t get_gyro_count(void) const { return _gyro_count; } bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; } bool gyro_calibrated_ok_all() const; - virtual bool get_accel_health(uint8_t instance) const { return true; } - bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } + bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; } + bool get_accel_health(void) const { return get_accel_health(_primary_accel); } bool get_accel_health_all(void) const; - virtual uint8_t get_accel_count(void) const { return 1; }; + uint8_t get_accel_count(void) const { return _accel_count; }; // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; } - const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(get_primary_accel()); } + const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); } // get accel scale const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; } - const Vector3f &get_accel_scale(void) const { return get_accel_scale(get_primary_accel()); } - - /* Update the sensor data, so that getters are nonblocking. - * Returns a bool of whether data was updated or not. - */ - virtual bool update() = 0; + const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); } /* get_delta_time returns the time period in seconds * overwhich the sensor data was collected */ - virtual float get_delta_time() const = 0; + float get_delta_time() const { return _delta_time; } // return the maximum gyro drift rate in radians/s/s. This // depends on what gyro chips are being used - virtual float get_gyro_drift_rate(void) = 0; + float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); } + + // update gyro and accel values from accumulated samples + void update(void); - // wait for a sample to be available, with timeout in milliseconds - virtual bool wait_for_sample(uint16_t timeout_ms) = 0; + // wait for a sample to be available + void wait_for_sample(void); // class level parameters static const struct AP_Param::GroupInfo var_info[]; @@ -169,24 +179,28 @@ class AP_InertialSensor } // get_filter - return filter in hz - virtual uint8_t get_filter() const { return _mpu6000_filter.get(); } + uint8_t get_filter() const { return _mpu6000_filter.get(); } - virtual uint16_t error_count(void) const { return 0; } - virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); } + // return the selected sample rate + Sample_rate get_sample_rate(void) const { return _sample_rate; } - virtual uint8_t get_primary_accel(void) const { return 0; } + uint16_t error_count(void) const { return 0; } + bool healthy(void) const { return get_gyro_health() && get_accel_health(); } -protected: + uint8_t get_primary_accel(void) const { return 0; } - virtual uint8_t _get_primary_gyro(void) const { return 0; } + // enable HIL mode + void set_hil_mode(void) { _hil_mode = true; } - // sensor specific init to be overwritten by descendant classes - virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0; +private: - // no-save implementations of accel and gyro initialisation routines - virtual void _init_accel(); + // load backend drivers + void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &)); + void _detect_backends(void); - virtual void _init_gyro(); + // accel and gyro initialisation + void _init_accel(); + void _init_gyro(); #if !defined( __AVR_ATmega1280__ ) // Calibration routines borrowed from Rolfe Schmidt @@ -194,53 +208,98 @@ class AP_InertialSensor // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // _calibrate_accel - perform low level accel calibration - virtual bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); - virtual void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); - virtual void _calibrate_reset_matrices(float dS[6], float JS[6][6]); - virtual void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); - virtual void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); + bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); + void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); + void _calibrate_reset_matrices(float dS[6], float JS[6][6]); + void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); + void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); #endif + // check if we have 3D accel calibration + void check_3D_calibration(void); + // save parameters to eeprom void _save_parameters(); - // Most recent accelerometer reading obtained by ::update - Vector3f _accel[INS_MAX_INSTANCES]; + // backend objects + AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS]; - // previous accelerometer reading obtained by ::update - Vector3f _previous_accel[INS_MAX_INSTANCES]; + // number of gyros and accel drivers. Note that most backends + // provide both accel and gyro data, so will increment both + // counters on initialisation + uint8_t _gyro_count; + uint8_t _accel_count; + uint8_t _backend_count; - // Most recent gyro reading obtained by ::update + // the selected sample rate + Sample_rate _sample_rate; + + // Most recent accelerometer reading + Vector3f _accel[INS_MAX_INSTANCES]; + + // Most recent gyro reading Vector3f _gyro[INS_MAX_INSTANCES]; // product id AP_Int16 _product_id; // accelerometer scaling and offsets - AP_Vector3f _accel_scale[INS_MAX_INSTANCES]; - AP_Vector3f _accel_offset[INS_MAX_INSTANCES]; - AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]; + AP_Vector3f _accel_scale[INS_MAX_INSTANCES]; + AP_Vector3f _accel_offset[INS_MAX_INSTANCES]; + AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]; // filtering frequency (0 means default) - AP_Int8 _mpu6000_filter; + AP_Int8 _mpu6000_filter; // board orientation from AHRS - enum Rotation _board_orientation; + enum Rotation _board_orientation; // calibrated_ok flags - bool _gyro_cal_ok[INS_MAX_INSTANCES]; + bool _gyro_cal_ok[INS_MAX_INSTANCES]; + + // primary accel and gyro + uint8_t _primary_gyro; + uint8_t _primary_accel; + + // has wait_for_sample() found a sample? + bool _have_sample:1; + + // are we in HIL mode? + bool _hil_mode:1; + + // do we have offsets/scaling from a 3D calibration? + bool _have_3D_calibration:1; + + // the delta time in seconds for the last sample + float _delta_time; + + // last time a wait_for_sample() returned a sample + uint32_t _last_sample_usec; + + // target time for next wait_for_sample() return + uint32_t _next_sample_usec; + + // time between samples in microseconds + uint32_t _sample_period_usec; + + // health of gyros and accels + bool _gyro_healthy[INS_MAX_INSTANCES]; + bool _accel_healthy[INS_MAX_INSTANCES]; + + uint32_t _accel_error_count[INS_MAX_INSTANCES]; + uint32_t _gyro_error_count[INS_MAX_INSTANCES]; }; -#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" -#include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_VRBRAIN.h" -#include "AP_InertialSensor_UserInteract_Stream.h" -#include "AP_InertialSensor_UserInteract_MAVLink.h" -#include "AP_InertialSensor_Flymaple.h" +#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_L3G4200D.h" +#include "AP_InertialSensor_Flymaple.h" #include "AP_InertialSensor_MPU9150.h" -#include "AP_InertialSensor_MPU9250.h" +#include "AP_InertialSensor_HIL.h" +#include "AP_InertialSensor_UserInteract_Stream.h" +#include "AP_InertialSensor_UserInteract_MAVLink.h" #endif // __AP_INERTIAL_SENSOR_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index ccc481c0863..684cb327e98 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -14,7 +14,7 @@ along with this program. If not, see . */ /* - Flymaple port by Mike McCauley + Flymaple IMU driver by Mike McCauley */ // Interface to the Flymaple sensors: @@ -28,20 +28,6 @@ const extern AP_HAL::HAL& hal; -/// Statics -Vector3f AP_InertialSensor_Flymaple::_accel_filtered; -uint32_t AP_InertialSensor_Flymaple::_accel_samples; -Vector3f AP_InertialSensor_Flymaple::_gyro_filtered; -uint32_t AP_InertialSensor_Flymaple::_gyro_samples; -uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp; -uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp; -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(800, 10); - // This is how often we wish to make raw samples of the sensors in Hz const uint32_t raw_sample_rate_hz = 800; // And the equivalent time between samples in microseconds @@ -77,25 +63,39 @@ const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz); // Result wil be radians/sec #define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f) -uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) +AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_gyro_sample(false), + _have_accel_sample(false), + _accel_filter_x(raw_sample_rate_hz, 10), + _accel_filter_y(raw_sample_rate_hz, 10), + _accel_filter_z(raw_sample_rate_hz, 10), + _gyro_filter_x(raw_sample_rate_hz, 10), + _gyro_filter_y(raw_sample_rate_hz, 10), + _gyro_filter_z(raw_sample_rate_hz, 10), + _last_gyro_timestamp(0), + _last_accel_timestamp(0) +{} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu) { - // Sensors are raw sampled at 800Hz. - // Here we figure the divider to get the rate that update should be called - switch (sample_rate) { - case RATE_50HZ: - _sample_divider = raw_sample_rate_hz / 50; - _default_filter_hz = 10; - break; - case RATE_100HZ: - _sample_divider = raw_sample_rate_hz / 100; - _default_filter_hz = 20; - break; - case RATE_200HZ: - default: - _sample_divider = raw_sample_rate_hz / 200; - _default_filter_hz = 20; - break; + AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu); + if (sensor == NULL) { + return NULL; } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_InertialSensor_Flymaple::_init_sensor(void) +{ + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -146,12 +146,17 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) hal.scheduler->delay(1); // Set up the filter desired - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); - // give back i2c semaphore + // give back i2c semaphore i2c_sem->give(); - return AP_PRODUCT_ID_FLYMAPLE; + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + + _product_id = AP_PRODUCT_ID_FLYMAPLE; + + return true; } /* @@ -170,109 +175,46 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz) _gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz); } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ // This takes about 20us to run bool AP_InertialSensor_Flymaple::update(void) { - if (!wait_for_sample(100)) { - return false; - } - Vector3f accel_scale = _accel_scale[0].get(); + Vector3f accel, gyro; - // Not really needed since Flymaple _accumulate runs in the main thread hal.scheduler->suspend_timer_procs(); - - // base the time on the gyro timestamp, as that is what is - // multiplied by time to integrate in DCM - _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f; - _last_update_usec = _last_gyro_timestamp; - - _previous_accel[0] = _accel[0]; - - _accel[0] = _accel_filtered; - _accel_samples = 0; - - _gyro[0] = _gyro_filtered; - _gyro_samples = 0; - + accel = _accel_filtered; + gyro = _gyro_filtered; + _have_gyro_sample = false; + _have_accel_sample = false; hal.scheduler->resume_timer_procs(); - // add offsets and rotation - _accel[0].rotate(_board_orientation); - // Adjust for chip scaling to get m/s/s - _accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; - - // Now the calibration scale factor - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - _gyro[0].rotate(_board_orientation); + accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; + _rotate_and_offset_accel(_accel_instance, accel); // Adjust for chip scaling to get radians/sec - _gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S; - _gyro[0] -= _gyro_offset[0]; + gyro *= FLYMAPLE_GYRO_SCALE_R_S; + _rotate_and_offset_gyro(_gyro_instance, gyro); - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } return true; } -bool AP_InertialSensor_Flymaple::get_gyro_health(void) const -{ - if (_last_gyro_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - uint64_t now = hal.scheduler->micros(); - if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) { - // gyros have not updated - return false; - } - return true; -} - -bool AP_InertialSensor_Flymaple::get_accel_health(void) const -{ - if (_last_accel_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - uint64_t now = hal.scheduler->micros(); - if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) { - // gyros have not updated - return false; - } - return true; -} - -float AP_InertialSensor_Flymaple::get_delta_time(void) const -{ - return _delta_time; -} - -float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void) -{ - // Dont really know this for the ITG-3200. - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - // This needs to get called as often as possible. // Its job is to accumulate samples as fast as is reasonable for the accel and gyro // sensors. -// Cant call this from within the system timers, since the long I2C reads (up to 1ms) -// with interrupts disabled breaks lots of things -// Therefore must call this as often as possible from -// within the mainline and thropttle the reads here to suit the sensors +// Note that this is called from gyro_sample_available() and +// accel_sample_available(), which is really not good enough for +// 800Hz, as it is common for the main loop to take more than 1.5ms +// before wait_for_sample() is called again. We can't just call this +// from a timer as timers run with interrupts disabled, and the I2C +// operations take too long +// So we are stuck with a suboptimal solution. The results are not so +// good in terms of timing. It may be better with the FIFOs enabled void AP_InertialSensor_Flymaple::_accumulate(void) { // get pointer to i2c bus semaphore @@ -285,7 +227,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) // Read accelerometer // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used uint8_t buffer[6]; - uint64_t now = hal.scheduler->micros(); + uint32_t now = hal.scheduler->micros(); // This takes about 250us at 400kHz I2C speed if ((now - _last_accel_timestamp) >= raw_sample_interval_us && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0) @@ -300,7 +242,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) _accel_filtered = Vector3f(_accel_filter_x.apply(x), _accel_filter_y.apply(y), _accel_filter_z.apply(z)); - _accel_samples++; + _have_accel_sample = true; _last_accel_timestamp = now; } @@ -317,7 +259,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) _gyro_filtered = Vector3f(_gyro_filter_x.apply(x), _gyro_filter_y.apply(y), _gyro_filter_z.apply(z)); - _gyro_samples++; + _have_gyro_sample = true; _last_gyro_timestamp = now; } @@ -325,26 +267,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void) i2c_sem->give(); } -bool AP_InertialSensor_Flymaple::_sample_available(void) -{ - _accumulate(); - return min(_accel_samples, _gyro_samples) / _sample_divider > 0; -} - -bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; -} - #endif // CONFIG_HAL_BOARD - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index b08546dd64d..724e03dbb49 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -6,39 +6,31 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE -#include #include "AP_InertialSensor.h" #include #include -class AP_InertialSensor_Flymaple : public AP_InertialSensor +class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend { public: + AP_InertialSensor_Flymaple(AP_InertialSensor &imu); - AP_InertialSensor_Flymaple() : AP_InertialSensor() {} + /* update accel and gyro state */ + bool update(); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); - bool get_gyro_health(void) const; - bool get_accel_health(void) const; - bool healthy(void) const { return get_gyro_health() && get_accel_health(); } + bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; } + bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - uint16_t _init_sensor( Sample_rate sample_rate ); - static void _accumulate(void); - bool _sample_available(); - uint64_t _last_update_usec; - float _delta_time; - static Vector3f _accel_filtered; - static uint32_t _accel_samples; - static Vector3f _gyro_filtered; - static uint32_t _gyro_samples; - static uint64_t _last_accel_timestamp; - static uint64_t _last_gyro_timestamp; - uint8_t _sample_divider; + bool _init_sensor(void); + void _accumulate(void); + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + bool _have_gyro_sample; + bool _have_accel_sample; // support for updating filter at runtime uint8_t _last_filter_hz; @@ -46,12 +38,18 @@ class AP_InertialSensor_Flymaple : public AP_InertialSensor void _set_filter_frequency(uint8_t filter_hz); // Low Pass filters for gyro and accel - static LowPassFilter2p _accel_filter_x; - static LowPassFilter2p _accel_filter_y; - static LowPassFilter2p _accel_filter_z; - static LowPassFilter2p _gyro_filter_x; - static LowPassFilter2p _gyro_filter_y; - static LowPassFilter2p _gyro_filter_z; + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; + + uint8_t _gyro_instance; + uint8_t _accel_instance; + + uint32_t _last_gyro_timestamp; + uint32_t _last_accel_timestamp; }; #endif #endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index bbb5e8d8ee7..50bfb4d6b6f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -1,130 +1,46 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "AP_InertialSensor_HIL.h" #include -const extern AP_HAL::HAL& hal; - -AP_InertialSensor_HIL::AP_InertialSensor_HIL() : - AP_InertialSensor(), - _sample_period_usec(0), - _last_sample_usec(0) -{ - _accel[0] = Vector3f(0, 0, -GRAVITY_MSS); -} - -uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) { - switch (sample_rate) { - case RATE_50HZ: - _sample_period_usec = 20000; - break; - case RATE_100HZ: - _sample_period_usec = 10000; - break; - case RATE_200HZ: - _sample_period_usec = 5000; - break; - case RATE_400HZ: - _sample_period_usec = 2500; - break; - } - return AP_PRODUCT_ID_NONE; -} - -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_HIL::update( void ) { - uint32_t now = hal.scheduler->micros(); - _last_sample_usec = now; - return true; -} - -float AP_InertialSensor_HIL::get_delta_time() const { - return _sample_period_usec * 1.0e-6f; -} - -float AP_InertialSensor_HIL::get_gyro_drift_rate(void) { - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} +#include "AP_InertialSensor_HIL.h" -bool AP_InertialSensor_HIL::_sample_available() -{ - uint32_t tnow = hal.scheduler->micros(); - bool have_sample = false; - while (tnow - _last_sample_usec > _sample_period_usec) { - have_sample = true; - _last_sample_usec += _sample_period_usec; - } - return have_sample; -} +const extern AP_HAL::HAL& hal; -bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) +AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu) { - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - uint32_t tnow = hal.scheduler->micros(); - uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow; - if (tdelay < 100000) { - hal.scheduler->delay_microseconds(tdelay); - } - if (_sample_available()) { - return true; - } - } - return false; } -void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel) +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu) { - if (instance >= INS_MAX_INSTANCES) { - return; + AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu); + if (sensor == NULL) { + return NULL; } - _previous_accel[instance] = _accel[instance]; - _accel[instance] = accel; - _last_accel_usec[instance] = hal.scheduler->micros(); -} - -void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro) -{ - if (instance >= INS_MAX_INSTANCES) { - return; + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; } - _gyro[instance] = gyro; - _last_gyro_usec[instance] = hal.scheduler->micros(); + return sensor; } -bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const +bool AP_InertialSensor_HIL::_init_sensor(void) { - if (instance >= INS_MAX_INSTANCES) { - return false; - } - return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000; -} + // grab the used instances + _imu.register_gyro(); + _imu.register_accel(); -bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const -{ - if (instance >= INS_MAX_INSTANCES) { - return false; - } - return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000; -} + _product_id = AP_PRODUCT_ID_NONE; + _imu.set_hil_mode(); -uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const -{ - if (get_gyro_health(1)) { - return 2; - } - return 1; + return true; } -uint8_t AP_InertialSensor_HIL::get_accel_count(void) const +bool AP_InertialSensor_HIL::update(void) { - if (get_accel_health(1)) { - return 2; - } - return 1; + // the data is stored directly in the frontend, so update() + // doesn't need to do anything + return true; } - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index 55ae828ad83..9c1422c662f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -1,36 +1,26 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#ifndef __AP_INERTIAL_SENSOR_STUB_H__ -#define __AP_INERTIAL_SENSOR_STUB_H__ +#ifndef __AP_INERTIALSENSOR_HIL_H__ +#define __AP_INERTIALSENSOR_HIL_H__ -#include #include "AP_InertialSensor.h" -class AP_InertialSensor_HIL : public AP_InertialSensor +class AP_InertialSensor_HIL : public AP_InertialSensor_Backend { public: + AP_InertialSensor_HIL(AP_InertialSensor &imu); - AP_InertialSensor_HIL(); + /* update accel and gyro state */ + bool update(); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); - void set_accel(uint8_t instance, const Vector3f &accel); - void set_gyro(uint8_t instance, const Vector3f &gyro); - bool get_gyro_health(uint8_t instance) const; - bool get_accel_health(uint8_t instance) const; - uint8_t get_gyro_count(void) const; - uint8_t get_accel_count(void) const; + bool gyro_sample_available(void) { return true; } + bool accel_sample_available(void) { return true; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - bool _sample_available(); - uint16_t _init_sensor( Sample_rate sample_rate ); - uint32_t _sample_period_usec; - uint32_t _last_sample_usec; - uint32_t _last_accel_usec[INS_MAX_INSTANCES]; - uint32_t _last_gyro_usec[INS_MAX_INSTANCES]; + bool _init_sensor(void); }; -#endif // __AP_INERTIAL_SENSOR_STUB_H__ +#endif // __AP_INERTIALSENSOR_HIL_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 60281bd5598..e1e4c5ce25b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -16,9 +16,17 @@ /* This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer. This combination is available as a cheap 10DOF sensor on ebay - */ -// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf -// L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf + + This sensor driver is an example only - it should not be used in any + serious autopilot as the latencies on I2C prevent good timing at + high sample rates. It is useful when doing an initial port of + ardupilot to a board where only I2C is available, and a cheap sensor + can be used. + +Datasheets: + ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf + L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf +*/ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX @@ -103,8 +111,10 @@ const extern AP_HAL::HAL& hal; #define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f) // constructor -AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() : - AP_InertialSensor(), +AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_gyro_sample(false), + _have_accel_sample(false), _accel_filter_x(800, 10), _accel_filter_y(800, 10), _accel_filter_z(800, 10), @@ -113,27 +123,9 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() : _gyro_filter_z(800, 10) {} -uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) +bool AP_InertialSensor_L3G4200D::_init_sensor(void) { - - switch (sample_rate) { - case RATE_50HZ: - _default_filter_hz = 10; - _sample_period_usec = (1000*1000) / 50; - _gyro_samples_needed = 16; - break; - case RATE_100HZ: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 100; - _gyro_samples_needed = 8; - break; - case RATE_200HZ: - default: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 200; - _gyro_samples_needed = 4; - break; - } + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -219,7 +211,7 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) // Set up the filter desired - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); // give back i2c semaphore i2c_sem->give(); @@ -227,15 +219,12 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate)); - clock_gettime(CLOCK_MONOTONIC, &_next_sample_ts); + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); - _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL; - if (_next_sample_ts.tv_nsec >= 1.0e9) { - _next_sample_ts.tv_nsec -= 1.0e9; - _next_sample_ts.tv_sec++; - } + _product_id = AP_PRODUCT_ID_L3G4200D; - return AP_PRODUCT_ID_L3G4200D; + return true; } /* @@ -254,58 +243,36 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz) _gyro_filter_z.set_cutoff_frequency(800, filter_hz); } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - +/* + copy filtered data to the frontend + */ bool AP_InertialSensor_L3G4200D::update(void) { - Vector3f accel_scale = _accel_scale[0].get(); - - _previous_accel[0] = _accel[0]; + Vector3f accel, gyro; hal.scheduler->suspend_timer_procs(); - _accel[0] = _accel_filtered; - _gyro[0] = _gyro_filtered; - _delta_time = _gyro_samples_available * (1.0f/800); - _gyro_samples_available = 0; + accel = _accel_filtered; + gyro = _gyro_filtered; + _have_gyro_sample = false; + _have_accel_sample = false; hal.scheduler->resume_timer_procs(); - // add offsets and rotation - _accel[0].rotate(_board_orientation); - // Adjust for chip scaling to get m/s/s - _accel[0] *= ADXL345_ACCELEROMETER_SCALE_M_S; - - // Now the calibration scale factor - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - _gyro[0].rotate(_board_orientation); + accel *= ADXL345_ACCELEROMETER_SCALE_M_S; + _rotate_and_offset_accel(_accel_instance, accel); // Adjust for chip scaling to get radians/sec - _gyro[0] *= L3G4200D_GYRO_SCALE_R_S; - _gyro[0] -= _gyro_offset[0]; + gyro *= L3G4200D_GYRO_SCALE_R_S; + _rotate_and_offset_gyro(_gyro_instance, gyro); - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } return true; } -float AP_InertialSensor_L3G4200D::get_delta_time(void) const -{ - return _delta_time; -} - -float AP_InertialSensor_L3G4200D::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute (a guess) - return ToRad(0.5/60); -} - // Accumulate values from accels and gyros void AP_InertialSensor_L3G4200D::_accumulate(void) { @@ -344,19 +311,17 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) _gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]), _gyro_filter_y.apply(-buffer[i][1]), _gyro_filter_z.apply(-buffer[i][2])); - _gyro_samples_available++; + _have_gyro_sample = true; } } } - // Read accelerometer FIFO to find out how many samples are available hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS, &fifo_status); num_samples_available = fifo_status & 0x3F; -#if 1 // read the samples and apply the filter if (num_samples_available > 0) { int16_t buffer[num_samples_available][3]; @@ -368,35 +333,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) _accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]), _accel_filter_y.apply(-buffer[i][1]), _accel_filter_z.apply(-buffer[i][2])); + _have_accel_sample = true; } } } -#endif // give back i2c semaphore i2c_sem->give(); } -bool AP_InertialSensor_L3G4200D::_sample_available(void) -{ - return (_gyro_samples_available >= _gyro_samples_needed); -} - -bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms) -{ - uint32_t start_us = hal.scheduler->micros(); - while (clock_nanosleep(CLOCK_MONOTONIC, - TIMER_ABSTIME, &_next_sample_ts, NULL) == -1 && errno == EINTR) ; - _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL; - if (_next_sample_ts.tv_nsec >= 1.0e9) { - _next_sample_ts.tv_nsec -= 1.0e9; - _next_sample_ts.tv_sec++; - - } - //_accumulate(); - return true; - -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 5aa035a6aa6..674dc87c15f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -6,40 +6,39 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include #include "AP_InertialSensor.h" #include #include -class AP_InertialSensor_L3G4200D : public AP_InertialSensor +class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend { public: - AP_InertialSensor_L3G4200D(); + AP_InertialSensor_L3G4200D(AP_InertialSensor &imu); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); + /* update accel and gyro state */ + bool update(); + + bool gyro_sample_available(void) { return _have_gyro_sample; } + bool accel_sample_available(void) { return _have_accel_sample; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); + + // return product ID + int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; } private: - uint16_t _init_sensor( Sample_rate sample_rate ); - void _accumulate(void); - bool _sample_available(); - uint64_t _last_update_usec; + bool _init_sensor(void); + void _accumulate(void); Vector3f _accel_filtered; Vector3f _gyro_filtered; - uint32_t _sample_period_usec; - uint32_t _last_sample_time; - volatile uint32_t _gyro_samples_available; - volatile uint8_t _gyro_samples_needed; - float _delta_time; - struct timespec _next_sample_ts; + volatile bool _have_gyro_sample; + volatile bool _have_accel_sample; // support for updating filter at runtime uint8_t _last_filter_hz; - uint8_t _default_filter_hz; + uint8_t _default_filter_hz; void _set_filter_frequency(uint8_t filter_hz); @@ -50,6 +49,10 @@ class AP_InertialSensor_L3G4200D : public AP_InertialSensor LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_z; + + // gyro and accel instances + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif #endif // __AP_INERTIAL_SENSOR_L3G4200D_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index a79e01dd99b..849851fb883 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -173,26 +173,51 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f); * variants however */ -AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : - AP_InertialSensor(), +AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), _drdy_pin(NULL), _spi(NULL), _spi_sem(NULL), - _num_samples(0), - _last_sample_time_micros(0), - _initialised(false), - _mpu6000_product_id(AP_PRODUCT_ID_NONE), - _sample_shift(0), _last_filter_hz(0), - _error_count(0) + _error_count(0), +#if MPU6000_FAST_SAMPLING + _accel_filter_x(1000, 15), + _accel_filter_y(1000, 15), + _accel_filter_z(1000, 15), + _gyro_filter_x(1000, 15), + _gyro_filter_y(1000, 15), + _gyro_filter_z(1000, 15), +#else + _sample_count(0), + _accel_sum(), + _gyro_sum(), +#endif + _sum_count(0) { } -uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu) { - if (_initialised) return _mpu6000_product_id; - _initialised = true; + AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +/* + initialise the sensor + */ +bool AP_InertialSensor_MPU6000::_init_sensor(void) +{ _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000); _spi_sem = _spi->get_semaphore(); @@ -205,103 +230,85 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) uint8_t tries = 0; do { - bool success = _hardware_init(sample_rate); + bool success = _hardware_init(); if (success) { hal.scheduler->delay(5+2); if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); + return false; } if (_data_ready()) { _spi_sem->give(); break; } else { - hal.console->println_P( - PSTR("MPU6000 startup failed: no data ready")); + return false; } _spi_sem->give(); } if (tries++ > 5) { - hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times")); + hal.console->print_P(PSTR("failed to boot MPU6000 5 times")); + return false; } } while (1); + // grab the used instances + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + hal.scheduler->resume_timer_procs(); - - /* read the first lot of data. - * _read_data_transaction requires the spi semaphore to be taken by - * its caller. */ - _last_sample_time_micros = hal.scheduler->micros(); - hal.scheduler->delay(10); - if (_spi_sem->take(100)) { - _read_data_transaction(); - _spi_sem->give(); - } - // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data)); #if MPU6000_DEBUG _dump_registers(); #endif - return _mpu6000_product_id; -} -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; + return true; } + +/* + process any + */ bool AP_InertialSensor_MPU6000::update( void ) -{ - // wait for at least 1 sample - if (!wait_for_sample(1000)) { +{ +#if !MPU6000_FAST_SAMPLING + if (_sum_count < _sample_count) { + // we don't have enough samples yet return false; } +#endif - _previous_accel[0] = _accel[0]; + // we have a full set of samples + uint16_t num_samples; + Vector3f accel, gyro; - // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); - _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); - _num_samples = _sum_count; +#if MPU6000_FAST_SAMPLING + gyro = _gyro_filtered; + accel = _accel_filtered; + num_samples = 1; +#else + gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); + accel(_accel_sum.x, _accel_sum.y, _accel_sum.z); + num_samples = _sum_count; _accel_sum.zero(); _gyro_sum.zero(); +#endif _sum_count = 0; hal.scheduler->resume_timer_procs(); - _gyro[0].rotate(_board_orientation); - _gyro[0] *= _gyro_scale / _num_samples; - _gyro[0] -= _gyro_offset[0]; - - _accel[0].rotate(_board_orientation); - _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; + gyro *= _gyro_scale / num_samples; + _rotate_and_offset_gyro(_gyro_instance, gyro); - Vector3f accel_scale = _accel_scale[0].get(); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; + accel *= MPU6000_ACCEL_SCALE_1G / num_samples; + _rotate_and_offset_accel(_accel_instance, accel); - if (_last_filter_hz != _mpu6000_filter) { + if (_last_filter_hz != _imu.get_filter()) { if (_spi_sem->take(10)) { _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - _set_filter_register(_mpu6000_filter, 0); + _set_filter_register(_imu.get_filter()); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _error_count = 0; _spi_sem->give(); } } @@ -331,35 +338,13 @@ bool AP_InertialSensor_MPU6000::_data_ready() */ void AP_InertialSensor_MPU6000::_poll_data(void) { - if (hal.scheduler->in_timerprocess()) { - if (!_spi_sem->take_nonblocking()) { - /* - the semaphore being busy is an expected condition when the - mainline code is calling wait_for_sample() which will - grab the semaphore. We return now and rely on the mainline - code grabbing the latest sample. - */ - return; - } - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - /* Synchronous read - take semaphore */ - if (_spi_sem->take(10)) { - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - hal.scheduler->panic( - PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data " - "failed to take SPI semaphore synchronously")); - } + if (!_spi_sem->take_nonblocking()) { + return; + } + if (_data_ready()) { + _read_data_transaction(); } + _spi_sem->give(); } @@ -390,19 +375,31 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() { } #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) +#if MPU6000_FAST_SAMPLING + _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), + _accel_filter_y.apply(int16_val(rx.v, 0)), + _accel_filter_z.apply(-int16_val(rx.v, 2))); + + _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), + _gyro_filter_y.apply(int16_val(rx.v, 4)), + _gyro_filter_z.apply(-int16_val(rx.v, 6))); +#else _accel_sum.x += int16_val(rx.v, 1); _accel_sum.y += int16_val(rx.v, 0); _accel_sum.z -= int16_val(rx.v, 2); _gyro_sum.x += int16_val(rx.v, 5); _gyro_sum.y += int16_val(rx.v, 4); _gyro_sum.z -= int16_val(rx.v, 6); +#endif _sum_count++; +#if !MPU6000_FAST_SAMPLING if (_sum_count == 0) { // rollover - v unlikely _accel_sum.zero(); _gyro_sum.zero(); } +#endif } uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg ) @@ -448,36 +445,30 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val) /* set the DLPF filter frequency. Assumes caller has taken semaphore */ -void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz) { - uint8_t filter = default_filter; + if (filter_hz == 0) { + filter_hz = _default_filter(); + } + uint8_t filter; // choose filtering frequency - switch (filter_hz) { - case 5: + if (filter_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; - break; - case 10: + } else if (filter_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; - break; - case 20: + } else if (filter_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; - break; - case 42: + } else if (filter_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; - break; - case 98: + } else { filter = BITS_DLPF_CFG_98HZ; - break; - } - - if (filter != 0) { - _last_filter_hz = filter_hz; - _register_write(MPUREG_CONFIG, filter); } + _last_filter_hz = filter_hz; + _register_write(MPUREG_CONFIG, filter); } -bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) +bool AP_InertialSensor_MPU6000::_hardware_init(void) { if (!_spi_sem->take(100)) { hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); @@ -519,46 +510,53 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); hal.scheduler->delay(1); - uint8_t default_filter; - +#if MPU6000_FAST_SAMPLING + _sample_count = 1; +#else // sample rate and filtering // to minimise the effects of aliasing we choose a filter // that is less than half of the sample rate - switch (sample_rate) { - case RATE_50HZ: + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: // this is used for plane and rover, where noise resistance is // more important than update rate. Tests on an aerobatic plane // show that 10Hz is fine, and makes it very noise resistant - default_filter = BITS_DLPF_CFG_10HZ; - _sample_shift = 2; + _sample_count = 4; break; - case RATE_100HZ: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 1; + case AP_InertialSensor::RATE_100HZ: + _sample_count = 2; break; - case RATE_200HZ: - default: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 0; + case AP_InertialSensor::RATE_200HZ: + _sample_count = 1; break; + default: + return false; } +#endif - _set_filter_register(_mpu6000_filter, default_filter); + _set_filter_register(_imu.get_filter()); +#if MPU6000_FAST_SAMPLING + // set sample rate to 1000Hz and apply a software filter + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ); +#else // set sample rate to 200Hz, and use _sample_divider to give // the requested rate to the application _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); +#endif hal.scheduler->delay(1); _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d - _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); + _product_id = _register_read(MPUREG_PRODUCT_ID); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || - (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { + if ((_product_id == MPU6000ES_REV_C4) || + (_product_id == MPU6000ES_REV_C5) || + (_product_id == MPU6000_REV_C4) || + (_product_id == MPU6000_REV_C5)) { // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D _register_write(MPUREG_ACCEL_CONFIG,1<<3); @@ -585,22 +583,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) return true; } -// return the MPU6k gyro drift rate in radian/s/s -// note that this is much better than the oilpan gyros -float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - -// return true if a sample is available -bool AP_InertialSensor_MPU6000::_sample_available() -{ - _poll_data(); - return (_sum_count >> _sample_shift) > 0; -} - - #if MPU6000_DEBUG // dump all config registers - used for debug void AP_InertialSensor_MPU6000::_dump_registers(void) @@ -619,11 +601,3 @@ void AP_InertialSensor_MPU6000::_dump_registers(void) } } #endif - - -// get_delta_time returns the time period in seconds overwhich the sensor data was collected -float AP_InertialSensor_MPU6000::get_delta_time() const -{ - // the sensor runs at 200Hz - return 0.005 * _num_samples; -} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index f6567d849da..3fa58d04c1f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -12,33 +12,44 @@ // enable debug to see a register dump on startup #define MPU6000_DEBUG 0 -class AP_InertialSensor_MPU6000 : public AP_InertialSensor +// on fast CPUs we sample at 1kHz and use a software filter +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 +#define MPU6000_FAST_SAMPLING 1 +#else +#define MPU6000_FAST_SAMPLING 0 +#endif + +#if MPU6000_FAST_SAMPLING +#include +#include +#endif + +class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend { public: + AP_InertialSensor_MPU6000(AP_InertialSensor &imu); - AP_InertialSensor_MPU6000(); - - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_gyro_drift_rate(); + /* update accel and gyro state */ + bool update(); - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); + bool gyro_sample_available(void) { return _sum_count >= _sample_count; } + bool accel_sample_available(void) { return _sum_count >= _sample_count; } - // get_delta_time returns the time period in seconds overwhich the sensor data was collected - float get_delta_time() const; + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - uint16_t error_count(void) const { return _error_count; } - bool healthy(void) const { return _error_count <= 4; } - bool get_gyro_health(uint8_t instance) const { return healthy(); } - bool get_accel_health(uint8_t instance) const { return healthy(); } +private: +#if MPU6000_DEBUG + void _dump_registers(void); +#endif -protected: - uint16_t _init_sensor( Sample_rate sample_rate ); + // instance numbers of accel and gyro data + uint8_t _gyro_instance; + uint8_t _accel_instance; -private: AP_HAL::DigitalSource *_drdy_pin; + bool _init_sensor(void); bool _sample_available(); void _read_data_transaction(); bool _data_ready(); @@ -46,41 +57,42 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor uint8_t _register_read( uint8_t reg ); void _register_write( uint8_t reg, uint8_t val ); void _register_write_check(uint8_t reg, uint8_t val); - bool _hardware_init(Sample_rate sample_rate); + bool _hardware_init(void); AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; - uint16_t _num_samples; static const float _gyro_scale; - uint32_t _last_sample_time_micros; - - // ensure we can't initialise twice - bool _initialised; - int16_t _mpu6000_product_id; - - // how many hardware samples before we report a sample to the caller - uint8_t _sample_shift; - // support for updating filter at runtime uint8_t _last_filter_hz; - void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + void _set_filter_register(uint8_t filter_hz); + // count of bus errors uint16_t _error_count; + // how many hardware samples before we report a sample to the caller + uint8_t _sample_count; + +#if MPU6000_FAST_SAMPLING + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + + // Low Pass filters for gyro and accel + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; +#else // accumulation in timer - must be read with timer disabled // the sum of the values since last read Vector3l _accel_sum; Vector3l _gyro_sum; - volatile int16_t _sum_count; - -public: - -#if MPU6000_DEBUG - void _dump_registers(void); #endif + volatile uint16_t _sum_count; }; #endif // __AP_INERTIAL_SENSOR_MPU6000_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index 44cd2b117a0..95130f12eb9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -19,6 +19,10 @@ Please check the following links for datasheets and documentation: - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf + + Note that this is an experimental driver. It is not used by any + actively maintained board and should be considered untested and + unmaintained */ #include @@ -320,19 +324,33 @@ static struct gyro_state_s st = { /** * @brief Constructor */ -AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() : - AP_InertialSensor(), +AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_sample_available(false), _accel_filter_x(800, 10), _accel_filter_y(800, 10), _accel_filter_z(800, 10), _gyro_filter_x(800, 10), _gyro_filter_y(800, 10), - _gyro_filter_z(800, 10) - // _mag_filter_x(800, 10), - // _mag_filter_y(800, 10), - // _mag_filter_z(800, 10) + _gyro_filter_z(800, 10) { +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; } /* @@ -352,41 +370,21 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz) } /** - * @brief Init method - * @param[in] Sample_rate The sample rate, check the struct def. - * @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful. + * Init method */ -uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) +bool AP_InertialSensor_MPU9150::_init_sensor(void) { // Sensors pushed to the FIFO. uint8_t sensors; - switch (sample_rate) { - case RATE_50HZ: - _default_filter_hz = 10; - _sample_period_usec = (1000*1000) / 50; - break; - case RATE_100HZ: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 100; - break; - case RATE_200HZ: - _default_filter_hz = 20; - _sample_period_usec = 5000; - break; - case RATE_400HZ: - default: - _default_filter_hz = 20; - _sample_period_usec = 2500; - break; - } + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){ - return -1; + return false; } // Init the sensor @@ -405,8 +403,8 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) // This registers are not documented in the register map. uint8_t buff[6]; if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) { - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision")); - goto failed; + hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"); + goto failed; } uint8_t rev; rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) | @@ -432,28 +430,28 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) // Set gyro full-scale range [250, 500, 1000, 2000] if (mpu_set_gyro_fsr(2000)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"); goto failed; } // Set the accel full-scale range if (mpu_set_accel_fsr(2)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"); goto failed; } // Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate) if (mpu_set_lpf(_default_filter_hz)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"); goto failed; } // Set sampling rate (value must be between 4Hz and 1KHz) if (mpu_set_sample_rate(800)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"); goto failed; } // Select which sensors are pushed to FIFO. sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO; if (mpu_configure_fifo(sensors)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"); goto failed; } @@ -467,18 +465,23 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) mpu_set_sensors(sensors); // Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp) - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); // give back i2c semaphore i2c_sem->give(); + + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate)); - return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE; - failed: - // give back i2c semaphore - i2c_sem->give(); - return -1; + return true; + +failed: + // give back i2c semaphore + i2c_sem->give(); + return false; } /** @@ -1017,9 +1020,9 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, * @brief Accumulate values from accels and gyros. * * This method is called periodically by the scheduler. - * */ -void AP_InertialSensor_MPU9150::_accumulate(void){ +void AP_InertialSensor_MPU9150::_accumulate(void) +{ // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -1094,102 +1097,36 @@ void AP_InertialSensor_MPU9150::_accumulate(void){ _gyro_filter_y.apply(gyro_y), _gyro_filter_z.apply(gyro_z)); - _gyro_samples_available++; + _have_sample_available = true; } // give back i2c semaphore i2c_sem->give(); } -bool AP_InertialSensor_MPU9150::_sample_available(void) -{ - uint64_t tnow = hal.scheduler->micros(); - while (tnow - _last_sample_timestamp > _sample_period_usec) { - _have_sample_available = true; - _last_sample_timestamp += _sample_period_usec; - } - return _have_sample_available; -} - -bool AP_InertialSensor_MPU9150::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - uint64_t tnow = hal.scheduler->micros(); - // we spin for the last timing_lag microseconds. Before that - // we yield the CPU to allow IO to happen - const uint16_t timing_lag = 400; - if (_last_sample_timestamp + _sample_period_usec > tnow+timing_lag) { - hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag)); - } - if (_sample_available()) { - return true; - } - } - return false; -} - bool AP_InertialSensor_MPU9150::update(void) { - if (!wait_for_sample(1000)) { - return false; - } - Vector3f accel_scale = _accel_scale[0].get(); + Vector3f accel, gyro; - _previous_accel[0] = _accel[0]; - - // hal.scheduler->suspend_timer_procs(); - _accel[0] = _accel_filtered; - _gyro[0] = _gyro_filtered; - // hal.scheduler->resume_timer_procs(); - - // add offsets and rotation - _accel[0].rotate(_board_orientation); - - // Adjust for chip scaling to get m/s/s - //////////////////////////////////////////////// - _accel[0] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available; - - // Now the calibration scale factor - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - _gyro[0].rotate(_board_orientation); + hal.scheduler->suspend_timer_procs(); + accel = _accel_filtered; + gyro = _gyro_filtered; + _have_sample_available = false; + hal.scheduler->resume_timer_procs(); - // Adjust for chip scaling to get radians/sec - _gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available; - _gyro[0] -= _gyro_offset[0]; - //////////////////////////////////////////////// + accel *= MPU9150_ACCEL_SCALE_2G; + _rotate_and_offset_accel(_accel_instance, accel); - _gyro_samples_available = 0; + gyro *= MPU9150_GYRO_SCALE_2000; + _rotate_and_offset_gyro(_gyro_instance, gyro); - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } - _have_sample_available = false; - return true; } -// TODO review to make sure it matches -float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute (a guess) - return ToRad(0.5/60); -} - -// TODO review to make sure it matches -float AP_InertialSensor_MPU9150::get_delta_time(void) const -{ - return _sample_period_usec * 1.0e-6f; -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h index a1d31fadab1..52de4f9d71f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h @@ -12,33 +12,30 @@ #include -class AP_InertialSensor_MPU9150 : public AP_InertialSensor +class AP_InertialSensor_MPU9150 : public AP_InertialSensor_Backend { public: + AP_InertialSensor_MPU9150(AP_InertialSensor &imu); - AP_InertialSensor_MPU9150(); + /* update accel and gyro state */ + bool update(); - /* Implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); + bool gyro_sample_available(void) { return _have_sample_available; } + bool accel_sample_available(void) { return _have_sample_available; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - uint16_t _init_sensor( Sample_rate sample_rate ); + bool _init_sensor(); void _accumulate(void); - bool _sample_available(); - // uint64_t _last_update_usec; Vector3f _accel_filtered; Vector3f _gyro_filtered; - uint32_t _sample_period_usec; - volatile uint32_t _gyro_samples_available; - uint64_t _last_sample_timestamp; - bool _have_sample_available; + bool _have_sample_available; // // support for updating filter at runtime uint8_t _last_filter_hz; - uint8_t _default_filter_hz; + uint8_t _default_filter_hz; int16_t mpu_set_gyro_fsr(uint16_t fsr); int16_t mpu_set_accel_fsr(uint8_t fsr); @@ -52,7 +49,6 @@ class AP_InertialSensor_MPU9150 : public AP_InertialSensor int16_t mpu_set_int_latched(uint8_t enable); int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more); - // Filter (specify which one) void _set_filter_frequency(uint8_t filter_hz); // Low Pass filters for gyro and accel @@ -62,11 +58,9 @@ class AP_InertialSensor_MPU9150 : public AP_InertialSensor LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_z; - // LowPassFilter2p _mag_filter_x; - // LowPassFilter2p _mag_filter_y; - // LowPassFilter2p _mag_filter_z; - + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif #endif // __AP_INERTIAL_SENSOR_MPU9150_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 6eefd8f2cd2..0c8cd321126 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -21,7 +21,6 @@ #include "AP_InertialSensor_MPU9250.h" #include "../AP_HAL_Linux/GPIO.h" -#include extern const AP_HAL::HAL& hal; @@ -158,29 +157,11 @@ extern const AP_HAL::HAL& hal; #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 - -// // TODO Remove -// // Product ID Description for MPU6000 -// // high 4 bits low 4 bits -// // Product Name Product Revision -// #define MPU6000ES_REV_C4 0x14 // 0001 0100 -// #define MPU6000ES_REV_C5 0x15 // 0001 0101 -// #define MPU6000ES_REV_D6 0x16 // 0001 0110 -// #define MPU6000ES_REV_D7 0x17 // 0001 0111 -// #define MPU6000ES_REV_D8 0x18 // 0001 1000 -// #define MPU6000_REV_C4 0x54 // 0101 0100 -// #define MPU6000_REV_C5 0x55 // 0101 0101 -// #define MPU6000_REV_D6 0x56 // 0101 0110 -// #define MPU6000_REV_D7 0x57 // 0101 0111 -// #define MPU6000_REV_D8 0x58 // 0101 1000 -// #define MPU6000_REV_D9 0x59 // 0101 1001 - - /* * PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) */ -const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); +#define GYRO_SCALE (0.0174532f / 16.4f) /* * PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of @@ -190,27 +171,48 @@ const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); * variants however */ -AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() : - AP_InertialSensor(), - _drdy_pin(NULL), - _initialised(false), - _mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE) +AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _last_filter_hz(-1), + _shared_data_idx(0), + _accel_filter_x(1000, 15), + _accel_filter_y(1000, 15), + _accel_filter_z(1000, 15), + _gyro_filter_x(1000, 15), + _gyro_filter_y(1000, 15), + _gyro_filter_z(1000, 15), + _have_sample_available(false) { } -uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu) { - if (_initialised) return _mpu9250_product_id; - _initialised = true; + AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +/* + initialise the sensor + */ +bool AP_InertialSensor_MPU9250::_init_sensor(void) +{ _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); _spi_sem = _spi->get_semaphore(); -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE - _drdy_pin = hal.gpio->channel(BBB_P8_7); - _drdy_pin->mode(HAL_GPIO_INPUT); -#endif - + // we need to suspend timers to prevent other SPI drivers grabbing + // the bus while we do the long initialisation hal.scheduler->suspend_timer_procs(); uint8_t whoami = _register_read(MPUREG_WHOAMI); @@ -218,43 +220,36 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) // TODO: we should probably accept multiple chip // revisions. This is the one on the PXF hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - hal.scheduler->panic("MPU9250: bad WHOAMI"); + return false; } uint8_t tries = 0; do { - bool success = _hardware_init(sample_rate); + bool success = _hardware_init(); if (success) { - hal.scheduler->delay(5+2); + hal.scheduler->delay(10); if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); + hal.console->printf("MPU9250: Unable to get semaphore"); + return false; } - if (_data_ready()) { + uint8_t status = _register_read(MPUREG_INT_STATUS); + if ((status & BIT_RAW_RDY_INT) != 0) { _spi_sem->give(); break; - } else { - hal.console->println_P( - PSTR("MPU9250 startup failed: no data ready")); } _spi_sem->give(); } if (tries++ > 5) { - hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times")); + return false; } } while (1); hal.scheduler->resume_timer_procs(); + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); - /* read the first lot of data. - * _read_data_transaction requires the spi semaphore to be taken by - * its caller. */ - _last_sample_time_micros = hal.scheduler->micros(); - hal.scheduler->delay(10); - if (_spi_sem->take(100)) { - _read_data_transaction(); - _spi_sem->give(); - } + _product_id = AP_PRODUCT_ID_MPU9250; // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data)); @@ -262,70 +257,45 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) #if MPU9250_DEBUG _dump_registers(); #endif - return _mpu9250_product_id; -} - -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; + return true; } +/* + update the accel and gyro vectors + */ bool AP_InertialSensor_MPU9250::update( void ) { - // wait for at least 1 sample - if (!wait_for_sample(1000)) { - return false; - } + // pull the data from the timer shared data buffer + uint8_t idx = _shared_data_idx; + Vector3f gyro = _shared_data[idx]._gyro_filtered; + Vector3f accel = _shared_data[idx]._accel_filtered; - _previous_accel[0] = _accel[0]; + _have_sample_available = false; - // disable timer procs for mininum time - hal.scheduler->suspend_timer_procs(); - _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); - _num_samples = _sum_count; - _accel_sum.zero(); - _gyro_sum.zero(); - _sum_count = 0; - hal.scheduler->resume_timer_procs(); + accel *= MPU9250_ACCEL_SCALE_1G; + gyro *= GYRO_SCALE; - _gyro[0].rotate(_board_orientation); - _gyro[0] *= _gyro_scale / _num_samples; - _gyro[0] -= _gyro_offset[0]; + // rotate for bbone default + accel.rotate(ROTATION_ROLL_180_YAW_90); + gyro.rotate(ROTATION_ROLL_180_YAW_90); + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + // PXF has an additional YAW 180 + accel.rotate(ROTATION_YAW_180); + gyro.rotate(ROTATION_YAW_180); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + // NavIO has different orientation, assuming RaspberryPi is right + // way up, and PWM pins on NavIO are at the back of the aircraft + accel.rotate(ROTATION_ROLL_180_YAW_90); + gyro.rotate(ROTATION_ROLL_180_YAW_90); +#endif - _accel[0].rotate(_board_orientation); - _accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples; + _rotate_and_offset_gyro(_gyro_instance, gyro); + _rotate_and_offset_accel(_accel_instance, accel); - // rotate for bbone default - _accel[0].rotate(ROTATION_ROLL_180_YAW_90); - _gyro[0].rotate(ROTATION_ROLL_180_YAW_90); - - Vector3f accel_scale = _accel_scale[0].get(); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - if (_last_filter_hz != _mpu6000_filter) { - if (_spi_sem->take(10)) { - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - _set_filter_register(_mpu6000_filter, 0); - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _error_count = 0; - _spi_sem->give(); - } + if (_last_filter_hz != _imu.get_filter()) { + _set_filter(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } return true; @@ -333,59 +303,30 @@ bool AP_InertialSensor_MPU9250::update( void ) /*================ HARDWARE FUNCTIONS ==================== */ -/** - * Return true if the MPU9250 has new data available for reading. - * - * We use the data ready pin if it is available. Otherwise, read the - * status register. - */ -bool AP_InertialSensor_MPU9250::_data_ready() -{ - if (_drdy_pin) { - return _drdy_pin->read() != 0; - } - uint8_t status = _register_read(MPUREG_INT_STATUS); - return (status & BIT_RAW_RDY_INT) != 0; -} - /** * Timer process to poll for new data from the MPU9250. */ void AP_InertialSensor_MPU9250::_poll_data(void) { - if (hal.scheduler->in_timerprocess()) { - if (!_spi_sem->take_nonblocking()) { - /* - the semaphore being busy is an expected condition when the - mainline code is calling wait_for_sample() which will - grab the semaphore. We return now and rely on the mainline - code grabbing the latest sample. - */ - return; - } - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - /* Synchronous read - take semaphore */ - if (_spi_sem->take(10)) { - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - hal.scheduler->panic( - PSTR("PANIC: AP_InertialSensor_MPU9250::_poll_data " - "failed to take SPI semaphore synchronously")); - } + if (!_spi_sem->take_nonblocking()) { + /* + the semaphore being busy is an expected condition when the + mainline code is calling wait_for_sample() which will + grab the semaphore. We return now and rely on the mainline + code grabbing the latest sample. + */ + return; } + _read_data_transaction(); + _spi_sem->give(); } -void AP_InertialSensor_MPU9250::_read_data_transaction() { +/* + read from the data registers and update filtered data + */ +void AP_InertialSensor_MPU9250::_read_data_transaction() +{ /* one resister address followed by seven 2-byte registers */ struct PACKED { uint8_t cmd; @@ -395,45 +336,27 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() { _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); - if (_drdy_pin) { - if (_drdy_pin->read() != 0) { - // data ready should have gone low after a read - printf("MPU9250: DRDY didn't go low\n"); - } - } +#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) - /* - detect a bad SPI bus transaction by looking for all 14 bytes - zero, or the wrong INT_STATUS register value. This is used to - detect a too high SPI bus speed. - */ - uint8_t i; - for (i=0; i<14; i++) { - if (rx.v[i] != 0) break; - } - if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) { - // likely a bad bus transaction - if (++_error_count > 4) { - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - } - } + Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), + _accel_filter_y.apply(int16_val(rx.v, 0)), + _accel_filter_z.apply(-int16_val(rx.v, 2))); -#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) - _accel_sum.x += int16_val(rx.v, 1); - _accel_sum.y += int16_val(rx.v, 0); - _accel_sum.z -= int16_val(rx.v, 2); - _gyro_sum.x += int16_val(rx.v, 5); - _gyro_sum.y += int16_val(rx.v, 4); - _gyro_sum.z -= int16_val(rx.v, 6); - _sum_count++; - - if (_sum_count == 0) { - // rollover - v unlikely - _accel_sum.zero(); - _gyro_sum.zero(); - } + Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), + _gyro_filter_y.apply(int16_val(rx.v, 4)), + _gyro_filter_z.apply(-int16_val(rx.v, 6))); + // update the shared buffer + uint8_t idx = _shared_data_idx ^ 1; + _shared_data[idx]._accel_filtered = _accel_filtered; + _shared_data[idx]._gyro_filtered = _gyro_filtered; + _shared_data_idx = idx; + + _have_sample_available = true; } +/* + read an 8 bit register + */ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) { uint8_t addr = reg | 0x80; // Set most significant bit @@ -444,10 +367,12 @@ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) tx[0] = addr; tx[1] = 0; _spi->transaction(tx, rx, 2); - return rx[1]; } +/* + write an 8 bit register + */ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) { uint8_t tx[2]; @@ -459,42 +384,32 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) } /* - set the DLPF filter frequency. Assumes caller has taken semaphore + set the accel/gyro filter frequency */ -void AP_InertialSensor_MPU9250::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz) { - uint8_t filter = default_filter; - // choose filtering frequency - switch (filter_hz) { - case 5: - filter = BITS_DLPF_CFG_5HZ; - break; - case 10: - filter = BITS_DLPF_CFG_10HZ; - break; - case 20: - filter = BITS_DLPF_CFG_20HZ; - break; - case 42: - filter = BITS_DLPF_CFG_42HZ; - break; - case 98: - filter = BITS_DLPF_CFG_98HZ; - break; + if (filter_hz == 0) { + filter_hz = _default_filter_hz; } - if (filter != 0) { - _last_filter_hz = filter_hz; + _accel_filter_x.set_cutoff_frequency(1000, filter_hz); + _accel_filter_y.set_cutoff_frequency(1000, filter_hz); + _accel_filter_z.set_cutoff_frequency(1000, filter_hz); - _register_write(MPUREG_CONFIG, filter); - } + _gyro_filter_x.set_cutoff_frequency(1000, filter_hz); + _gyro_filter_y.set_cutoff_frequency(1000, filter_hz); + _gyro_filter_z.set_cutoff_frequency(1000, filter_hz); } -bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) +/* + initialise the sensor configuration registers + */ +bool AP_InertialSensor_MPU9250::_hardware_init(void) { if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); + hal.console->printf("MPU9250: Unable to get semaphore"); + return false; } // initially run the bus at low speed @@ -503,7 +418,11 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) // Chip reset uint8_t tries; for (tries = 0; tries<5; tries++) { +#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 + /* Prevent reseting if internal AK8963 is selected, because it may corrupt + * AK8963's initialisation. */ _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); +#endif hal.scheduler->delay(100); // Wake up device and select GyroZ clock. Note that the @@ -527,68 +446,30 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) } _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode - hal.scheduler->delay(1); // Disable I2C bus (recommended on datasheet) +#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 + /* Prevent disabling if internal AK8963 is selected. If internal AK8963 is not used + * it's ok to disable I2C slaves*/ _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); - hal.scheduler->delay(1); - - uint8_t default_filter; - - // sample rate and filtering - // to minimise the effects of aliasing we choose a filter - // that is less than half of the sample rate - switch (sample_rate) { - case RATE_50HZ: - // this is used for plane and rover, where noise resistance is - // more important than update rate. Tests on an aerobatic plane - // show that 10Hz is fine, and makes it very noise resistant - default_filter = BITS_DLPF_CFG_10HZ; - _sample_shift = 2; - break; - case RATE_100HZ: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 1; - break; - case RATE_200HZ: - default: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 0; - break; - } +#endif - _set_filter_register(_mpu6000_filter, default_filter); + _default_filter_hz = _default_filter(); - // set sample rate to 200Hz, and use _sample_divider to give - // the requested rate to the application - _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); - hal.scheduler->delay(1); + // used a fixed filter of 42Hz on the sensor, then filter using + // the 2-pole software filter + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); + // set sample rate to 1kHz, and use the 2 pole filter to give the + // desired rate + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ); _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s - hal.scheduler->delay(1); - - // // read the product ID rev c has 1/2 the sensitivity of rev d - // _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); - // //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - - // if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || - // (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { - // // Accel scale 8g (4096 LSB/g) - // // Rev C has different scaling than rev D - // _register_write(MPUREG_ACCEL_CONFIG,1<<3); - // } else { - // // Accel scale 8g (4096 LSB/g) - // _register_write(MPUREG_ACCEL_CONFIG,2<<3); - // } // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g _register_write(MPUREG_ACCEL_CONFIG,2<<3); - hal.scheduler->delay(1); - // configure interrupt to fire when new data arrives _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); - hal.scheduler->delay(1); // clear interrupt on any read, and hold the data ready pin high // until we clear the interrupt @@ -603,22 +484,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) return true; } -// return the MPUXk gyro drift rate in radian/s/s -// note that this is much better than the oilpan gyros -float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - -// return true if a sample is available -bool AP_InertialSensor_MPU9250::_sample_available() -{ - _poll_data(); - return (_sum_count >> _sample_shift) > 0; -} - - #if MPU9250_DEBUG // dump all config registers - used for debug void AP_InertialSensor_MPU9250::_dump_registers(void) @@ -636,12 +501,4 @@ void AP_InertialSensor_MPU9250::_dump_registers(void) #endif -// get_delta_time returns the time period in seconds overwhich the sensor data was collected -float AP_InertialSensor_MPU9250::get_delta_time() const -{ - // the sensor runs at 200Hz - return 0.005 * _num_samples; - -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index aeb1dd998c7..1ce5b1a67fe 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -7,75 +7,75 @@ #include #include #include +#include +#include #include "AP_InertialSensor.h" // enable debug to see a register dump on startup #define MPU9250_DEBUG 0 -class AP_InertialSensor_MPU9250 : public AP_InertialSensor +class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend { public: - AP_InertialSensor_MPU9250(); + AP_InertialSensor_MPU9250(AP_InertialSensor &imu); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_gyro_drift_rate(); + /* update accel and gyro state */ + bool update(); - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); + bool gyro_sample_available(void) { return _have_sample_available; } + bool accel_sample_available(void) { return _have_sample_available; } - // get_delta_time returns the time period in seconds overwhich the sensor data was collected - float get_delta_time() const; - - uint16_t error_count(void) const { return _error_count; } - bool healthy(void) const { return _error_count <= 4; } - bool get_gyro_health(uint8_t instance) const { return healthy(); } - bool get_accel_health(uint8_t instance) const { return healthy(); } - -protected: - uint16_t _init_sensor( Sample_rate sample_rate ); + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - AP_HAL::DigitalSource *_drdy_pin; + bool _init_sensor(void); - bool _sample_available(); void _read_data_transaction(); bool _data_ready(); void _poll_data(void); uint8_t _register_read( uint8_t reg ); void _register_write( uint8_t reg, uint8_t val ); - bool _hardware_init(Sample_rate sample_rate); + bool _hardware_init(void); + bool _sample_available(); AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; - uint16_t _num_samples; - static const float _gyro_scale; - - uint32_t _last_sample_time_micros; - - // ensure we can't initialise twice - bool _initialised; - int16_t _mpu9250_product_id; - - // how many hardware samples before we report a sample to the caller - uint8_t _sample_shift; - // support for updating filter at runtime - uint8_t _last_filter_hz; - - void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); - - uint16_t _error_count; - - // accumulation in timer - must be read with timer disabled - // the sum of the values since last read - Vector3l _accel_sum; - Vector3l _gyro_sum; - volatile int16_t _sum_count; - -public: + int16_t _last_filter_hz; + + // change the filter frequency + void _set_filter(uint8_t filter_hz); + + // This structure is used to pass data from the timer which reads + // the sensor to the main thread. The _shared_data_idx is used to + // prevent race conditions by ensuring the data is fully updated + // before being used by the consumer + struct { + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + } _shared_data[2]; + volatile uint8_t _shared_data_idx; + + // Low Pass filters for gyro and accel + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; + + // do we currently have a sample pending? + bool _have_sample_available; + + // default filter frequency when set to zero + uint8_t _default_filter_hz; + + // gyro and accel instances + uint8_t _gyro_instance; + uint8_t _accel_instance; #if MPU9250_DEBUG void _dump_registers(void); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 7070ff8e87a..df56b447464 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -1,11 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include + #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #include "AP_InertialSensor_Oilpan.h" +#include const extern AP_HAL::HAL& hal; +// this driver assumes an AP_ADC object has been declared globally +extern AP_ADC_ADS7844 apm1_adc; + // ADC channel mappings on for the APM Oilpan // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; @@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = { 1, -1, -1, 1, -1, -1 }; -// ADC channel reading the gyro temperature -const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3; - // Maximum possible value returned by an offset-corrected sensor channel const float AP_InertialSensor_Oilpan::_adc_constraint = 900; @@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900; #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) #define OILPAN_RAW_GYRO_OFFSET 1658.0f -#define ToRad(x) radians(x) // *pi/180 // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 -const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f); -const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f); -const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f); +const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f); /* ------ Public functions -------------------------------------------*/ -AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : - AP_InertialSensor(), - _adc(adc) +AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu) +{ +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu) { + AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + + return sensor; } -uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate) +bool AP_InertialSensor_Oilpan::_init_sensor(void) { - _adc->Init(); + apm1_adc.Init(); - switch (sample_rate) { - case RATE_50HZ: + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: _sample_threshold = 20; break; - case RATE_100HZ: + case AP_InertialSensor::RATE_100HZ: _sample_threshold = 10; break; - case RATE_200HZ: + case AP_InertialSensor::RATE_200HZ: _sample_threshold = 5; break; + default: + // can't do this speed + return false; } -#if defined(__AVR_ATmega1280__) - return AP_PRODUCT_ID_APM1_1280; -#else - return AP_PRODUCT_ID_APM1_2560; -#endif + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + + _product_id = AP_PRODUCT_ID_APM1_2560; + + return true; } +/* + copy data from ADC to frontend + */ bool AP_InertialSensor_Oilpan::update() { - if (!wait_for_sample(100)) { - return false; - } float adc_values[6]; - Vector3f gyro_offset = _gyro_offset[0].get(); - Vector3f accel_scale = _accel_scale[0].get(); - Vector3f accel_offset = _accel_offset[0].get(); - - _delta_time_micros = _adc->Ch6(_sensors, adc_values); - _temp = _adc->Ch(_gyro_temp_ch); - - _gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET )); - _gyro[0].rotate(_board_orientation); - _gyro[0].x *= _gyro_gain_x; - _gyro[0].y *= _gyro_gain_y; - _gyro[0].z *= _gyro_gain_z; - _gyro[0] -= gyro_offset; - - _previous_accel[0] = _accel[0]; - - _accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); - _accel[0].rotate(_board_orientation); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] *= OILPAN_ACCEL_SCALE_1G; - _accel[0] -= accel_offset; - -/* - * X = 1619.30 to 2445.69 - * Y = 1609.45 to 2435.42 - * Z = 1627.44 to 2434.82 - */ - return true; -} + apm1_adc.Ch6(_sensors, adc_values); -float AP_InertialSensor_Oilpan::get_delta_time() const { - return _delta_time_micros * 1.0e-6; -} + // copy gyros to frontend + Vector3f v; + v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x, + _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y, + _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z); + _rotate_and_offset_gyro(_gyro_instance, v); -/* ------ Private functions -------------------------------------------*/ + // copy accels to frontend + v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); + v *= OILPAN_ACCEL_SCALE_1G; + _rotate_and_offset_accel(_accel_instance, v); -// return the oilpan gyro drift rate in radian/s/s -float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) -{ - // 3.0 degrees/second/minute - return ToRad(3.0/60); + return true; } // return true if a new sample is available -bool AP_InertialSensor_Oilpan::_sample_available() -{ - return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; -} - -bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms) +bool AP_InertialSensor_Oilpan::_sample_available() const { - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; + return apm1_adc.num_samples_available(_sensors) >= _sample_threshold; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index bffb4f21146..e00169424c6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -3,50 +3,35 @@ #ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ #define __AP_INERTIAL_SENSOR_OILPAN_H__ -#include - -#include -#include +#include #include "AP_InertialSensor.h" -class AP_InertialSensor_Oilpan : public AP_InertialSensor +class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend { public: + AP_InertialSensor_Oilpan(AP_InertialSensor &imu); - AP_InertialSensor_Oilpan( AP_ADC * adc ); - - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); + /* update accel and gyro state */ + bool update(); - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); + bool gyro_sample_available(void) { return _sample_available(); } + bool accel_sample_available(void) { return _sample_available(); } -protected: - uint16_t _init_sensor(Sample_rate sample_rate); + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - - bool _sample_available(); - - AP_ADC * _adc; - - float _temp; - - uint32_t _delta_time_micros; - + bool _init_sensor(void); + bool _sample_available() const; static const uint8_t _sensors[6]; static const int8_t _sensor_signs[6]; - static const uint8_t _gyro_temp_ch; - static const float _gyro_gain_x; static const float _gyro_gain_y; static const float _gyro_gain_z; - static const float _adc_constraint; - uint8_t _sample_threshold; + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif // __AP_INERTIAL_SENSOR_OILPAN_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 4f473f5637e..c36e1264141 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -1,7 +1,8 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + #include "AP_InertialSensor_PX4.h" const extern AP_HAL::HAL& hal; @@ -15,11 +16,33 @@ const extern AP_HAL::HAL& hal; #include #include -#include +#include + +AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _last_get_sample_timestamp(0) +{ +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} -uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) +bool AP_InertialSensor_PX4::_init_sensor(void) { - // assumes max 2 instances + // assumes max 3 instances _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY); _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY); _accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY); @@ -32,45 +55,30 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) for (uint8_t i=0; i= 0) { _num_accel_instances = i+1; + _accel_instance[i] = _imu.register_accel(); } if (_gyro_fd[i] >= 0) { _num_gyro_instances = i+1; + _gyro_instance[i] = _imu.register_gyro(); } } if (_num_accel_instances == 0) { - hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); + return false; } if (_num_gyro_instances == 0) { - hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); - } - - switch (sample_rate) { - case RATE_50HZ: - _default_filter_hz = 15; - _sample_time_usec = 20000; - break; - case RATE_100HZ: - _default_filter_hz = 30; - _sample_time_usec = 10000; - break; - case RATE_200HZ: - _default_filter_hz = 30; - _sample_time_usec = 5000; - break; - case RATE_400HZ: - default: - _default_filter_hz = 30; - _sample_time_usec = 2500; - break; + return false; } - _set_filter_frequency(_mpu6000_filter); + _default_filter_hz = _default_filter(); + _set_filter_frequency(_imu.get_filter()); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - return AP_PRODUCT_ID_PX4_V2; + _product_id = AP_PRODUCT_ID_PX4_V2; #else - return AP_PRODUCT_ID_PX4; + _product_id = AP_PRODUCT_ID_PX4; #endif + + return true; } /* @@ -89,109 +97,39 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz) } } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -// multi-device interface -bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const -{ - if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - if (instance >= _num_gyro_instances) { - return false; - } - - if ((_last_get_sample_timestamp - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) { - // gyros have not updated - return false; - } - return true; -} - -uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const -{ - return _num_gyro_instances; -} - -bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const -{ - if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - if (k >= _num_accel_instances) { - return false; - } - - if ((_last_get_sample_timestamp - _last_accel_timestamp[k]) > 2*_sample_time_usec) { - // accels have not updated - return false; - } - if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 && - (_previous_accel[k] - _accel[k]).length() < 0.01f) { - // unchanging accel, large in all 3 axes. This is a likely - // accelerometer failure of the LSM303d - return false; - } - return true; - -} - -uint8_t AP_InertialSensor_PX4::get_accel_count(void) const -{ - return _num_accel_instances; -} - bool AP_InertialSensor_PX4::update(void) { - if (!wait_for_sample(100)) { - return false; - } - // get the latest sample from the sensor drivers _get_sample(); - for (uint8_t k=0; k<_num_accel_instances; k++) { - _previous_accel[k] = _accel[k]; - _accel[k] = _accel_in[k]; - _accel[k].rotate(_board_orientation); - _accel[k].x *= _accel_scale[k].get().x; - _accel[k].y *= _accel_scale[k].get().y; - _accel[k].z *= _accel_scale[k].get().z; - _accel[k] -= _accel_offset[k]; + Vector3f accel = _accel_in[k]; + // calling _rotate_and_offset_accel sets the sensor healthy, + // so we only want to do this if we have new data from it + if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) { + _rotate_and_offset_accel(_accel_instance[k], accel); + _last_accel_update_timestamp[k] = _last_accel_timestamp[k]; + } } for (uint8_t k=0; k<_num_gyro_instances; k++) { - _gyro[k] = _gyro_in[k]; - _gyro[k].rotate(_board_orientation); - _gyro[k] -= _gyro_offset[k]; + Vector3f gyro = _gyro_in[k]; + // calling _rotate_and_offset_accel sets the sensor healthy, + // so we only want to do this if we have new data from it + if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) { + _rotate_and_offset_gyro(_gyro_instance[k], gyro); + _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k]; + } } - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } - _have_sample_available = false; - return true; } -float AP_InertialSensor_PX4::get_delta_time(void) const -{ - return _sample_time_usec * 1.0e-6f; -} - -float AP_InertialSensor_PX4::get_gyro_drift_rate(void) -{ - // assume 0.5 degrees/second/minute - return ToRad(0.5/60); -} - void AP_InertialSensor_PX4::_get_sample(void) { for (uint8_t i=0; i<_num_accel_instances; i++) { @@ -201,6 +139,7 @@ void AP_InertialSensor_PX4::_get_sample(void) accel_report.timestamp != _last_accel_timestamp[i]) { _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z); _last_accel_timestamp[i] = accel_report.timestamp; + _set_accel_error_count(_accel_instance[i], accel_report.error_count); } } for (uint8_t i=0; i<_num_gyro_instances; i++) { @@ -210,64 +149,32 @@ void AP_InertialSensor_PX4::_get_sample(void) gyro_report.timestamp != _last_gyro_timestamp[i]) { _gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); _last_gyro_timestamp[i] = gyro_report.timestamp; + _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count); } } - _last_get_sample_timestamp = hrt_absolute_time(); + _last_get_sample_timestamp = hal.scheduler->micros64(); } -bool AP_InertialSensor_PX4::_sample_available(void) +bool AP_InertialSensor_PX4::gyro_sample_available(void) { - uint64_t tnow = hrt_absolute_time(); - while (tnow - _last_sample_timestamp > _sample_time_usec) { - _have_sample_available = true; - _last_sample_timestamp += _sample_time_usec; - } - return _have_sample_available; -} - -bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - uint64_t tnow = hrt_absolute_time(); - // we spin for the last timing_lag microseconds. Before that - // we yield the CPU to allow IO to happen - const uint16_t timing_lag = 400; - if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) { - hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag)); - } - if (_sample_available()) { + _get_sample(); + for (uint8_t i=0; i<_num_gyro_instances; i++) { + if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) { return true; } - } - return false; -} - -/** - try to detect bad accel/gyro sensors - */ -bool AP_InertialSensor_PX4::healthy(void) const -{ - return get_gyro_health(0) && get_accel_health(0); -} - -uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const -{ - for (uint8_t i=0; i<_num_gyro_instances; i++) { - if (get_gyro_health(i)) return i; } - return 0; + return false; } -uint8_t AP_InertialSensor_PX4::get_primary_accel(void) const +bool AP_InertialSensor_PX4::accel_sample_available(void) { + _get_sample(); for (uint8_t i=0; i<_num_accel_instances; i++) { - if (get_accel_health(i)) return i; + if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) { + return true; + } } - return 0; + return false; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 1950910c4ec..262608baaa2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -4,7 +4,7 @@ #define __AP_INERTIAL_SENSOR_PX4_H__ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include #include "AP_InertialSensor.h" @@ -13,47 +13,33 @@ #include #include -class AP_InertialSensor_PX4 : public AP_InertialSensor +class AP_InertialSensor_PX4 : public AP_InertialSensor_Backend { public: - AP_InertialSensor_PX4() : - AP_InertialSensor(), - _last_get_sample_timestamp(0), - _sample_time_usec(0) - { - } + AP_InertialSensor_PX4(AP_InertialSensor &imu); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); - bool healthy(void) const; + /* update accel and gyro state */ + bool update(); - // multi-device interface - bool get_gyro_health(uint8_t instance) const; - uint8_t get_gyro_count(void) const; + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - bool get_accel_health(uint8_t instance) const; - uint8_t get_accel_count(void) const; - - uint8_t get_primary_accel(void) const; + bool gyro_sample_available(void); + bool accel_sample_available(void); private: - uint8_t _get_primary_gyro(void) const; - - uint16_t _init_sensor( Sample_rate sample_rate ); + bool _init_sensor(void); void _get_sample(void); bool _sample_available(void); Vector3f _accel_in[INS_MAX_INSTANCES]; Vector3f _gyro_in[INS_MAX_INSTANCES]; uint64_t _last_accel_timestamp[INS_MAX_INSTANCES]; uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES]; uint64_t _last_get_sample_timestamp; uint64_t _last_sample_timestamp; - uint32_t _sample_time_usec; - bool _have_sample_available; // support for updating filter at runtime uint8_t _last_filter_hz; @@ -64,8 +50,14 @@ class AP_InertialSensor_PX4 : public AP_InertialSensor // accelerometer and gyro driver handles uint8_t _num_accel_instances; uint8_t _num_gyro_instances; + int _accel_fd[INS_MAX_INSTANCES]; int _gyro_fd[INS_MAX_INSTANCES]; + + // indexes in frontend object. Note that these could be different + // from the backend indexes + uint8_t _accel_instance[INS_MAX_INSTANCES]; + uint8_t _gyro_instance[INS_MAX_INSTANCES]; }; -#endif +#endif // CONFIG_HAL_BOARD #endif // __AP_INERTIAL_SENSOR_PX4_H__ diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde index c28dc8e7d2b..81b0e7bcc0c 100644 --- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde +++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde @@ -36,28 +36,12 @@ #include #include #include +#include +#include const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -#define CONFIG_INS_TYPE HAL_INS_DEFAULT - -#if CONFIG_INS_TYPE == HAL_INS_MPU6000 -AP_InertialSensor_MPU6000 ins; -#elif CONFIG_INS_TYPE == HAL_INS_PX4 -AP_InertialSensor_PX4 ins; -#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN -AP_InertialSensor_VRBRAIN ins; -#elif CONFIG_INS_TYPE == HAL_INS_HIL -AP_InertialSensor_HIL ins; -#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE -AP_InertialSensor_Flymaple ins; -#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D -AP_InertialSensor_L3G4200D ins; -#elif CONFIG_INS_TYPE == HAL_INS_MPU9250 -AP_InertialSensor_MPU9250 ins; -#else - #error Unrecognised CONFIG_INS_TYPE setting. -#endif // CONFIG_INS_TYPE +AP_InertialSensor ins; void setup(void) { @@ -208,7 +192,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - ins.wait_for_sample(1000); + ins.wait_for_sample(); // read samples from ins ins.update(); From 94f3c64df950547cdd75a770d2f5044f8f550aea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:19:29 +1100 Subject: [PATCH 217/254] HAL_SITL: fixed for changes in AP_InertialSensor API --- libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 4 ++-- libraries/AP_HAL_AVR_SITL/SITL_State.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index 01c06c1fdfc..1155f3127c0 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -65,7 +65,7 @@ uint16_t SITL_State::current_pin_value; float SITL_State::_current; AP_Baro_HIL *SITL_State::_barometer; -AP_InertialSensor_HIL *SITL_State::_ins; +AP_InertialSensor *SITL_State::_ins; SITLScheduler *SITL_State::_scheduler; AP_Compass_HIL *SITL_State::_compass; @@ -212,7 +212,7 @@ void SITL_State::_sitl_setup(void) // find the barometer object if it exists _sitl = (SITL *)AP_Param::find_object("SIM_"); _barometer = (AP_Baro_HIL *)AP_Param::find_object("GND_"); - _ins = (AP_InertialSensor_HIL *)AP_Param::find_object("INS_"); + _ins = (AP_InertialSensor *)AP_Param::find_object("INS_"); _compass = (AP_Compass_HIL *)AP_Param::find_object("COMPASS_"); if (_sitl != NULL) { diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index 8d7b74471e3..ca5c8e1ef22 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -121,7 +121,7 @@ class AVR_SITL::SITL_State { static bool _motors_on; static AP_Baro_HIL *_barometer; - static AP_InertialSensor_HIL *_ins; + static AP_InertialSensor *_ins; static SITLScheduler *_scheduler; static AP_Compass_HIL *_compass; From 7b178efd25d19a0f520e069e68cb409b8fccfd34 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:19:40 +1100 Subject: [PATCH 218/254] Copter: fixes for AP_InertialSensor API changes --- ArduCopter/ArduCopter.pde | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f03d2a5bbf0..54f0adacaa4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -291,25 +291,7 @@ static AP_Compass_HIL compass; AP_ADC_ADS7844 apm1_adc; #endif -#if CONFIG_INS_TYPE == HAL_INS_MPU6000 -AP_InertialSensor_MPU6000 ins; -#elif CONFIG_INS_TYPE == HAL_INS_PX4 -AP_InertialSensor_PX4 ins; -#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN -AP_InertialSensor_VRBRAIN ins; -#elif CONFIG_INS_TYPE == HAL_INS_HIL -AP_InertialSensor_HIL ins; -#elif CONFIG_INS_TYPE == HAL_INS_OILPAN -AP_InertialSensor_Oilpan ins( &apm1_adc ); -#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE -AP_InertialSensor_Flymaple ins; -#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D -AP_InertialSensor_L3G4200D ins; -#elif CONFIG_INS_TYPE == HAL_INS_MPU9250 -AP_InertialSensor_MPU9250 ins; -#else - #error Unrecognised CONFIG_INS_TYPE setting. -#endif // CONFIG_INS_TYPE +AP_InertialSensor ins; // Inertial Navigation EKF #if AP_AHRS_NAVEKF_AVAILABLE @@ -963,10 +945,7 @@ static void perf_update(void) void loop() { // wait for an INS sample - if (!ins.wait_for_sample(1000)) { - Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY); - return; - } + ins.wait_for_sample(); uint32_t timer = micros(); // check loop time From fbef07c08d333eee694647e93326403ae377c392 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:20:13 +1100 Subject: [PATCH 219/254] Travis: added build script from master --- Tools/scripts/build_all_travis.sh | 38 +++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100755 Tools/scripts/build_all_travis.sh diff --git a/Tools/scripts/build_all_travis.sh b/Tools/scripts/build_all_travis.sh new file mode 100755 index 00000000000..b8e2e7e503b --- /dev/null +++ b/Tools/scripts/build_all_travis.sh @@ -0,0 +1,38 @@ +#!/bin/bash +# useful script to test all the different build types that we support. +# This helps when doing large merges +# Andrew Tridgell, November 2011 + +. config.mk + +set -e +set -x + +echo "Testing ArduPlane build" +pushd ArduPlane +make configure +for b in all apm2 sitl linux; do + pwd + make clean + make $b -j4 +done +popd + +for d in ArduCopter APMrover2 ArduPlane AntennaTracker; do + pushd $d + make clean + make sitl -j4 + make clean + make linux -j4 + make clean + make px4-cleandep + make px4-v2 + popd +done + +pushd Tools/Replay +make clean +make linux -j4 +popd + +exit 0 From 9b9665153e9f16012c555630cdc32e643da239a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:21:13 +1100 Subject: [PATCH 220/254] Travis: update to latest prereqs script from master --- Tools/scripts/install-prereqs-ubuntu.sh | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/Tools/scripts/install-prereqs-ubuntu.sh b/Tools/scripts/install-prereqs-ubuntu.sh index b5a319b3868..3517a6a7cff 100755 --- a/Tools/scripts/install-prereqs-ubuntu.sh +++ b/Tools/scripts/install-prereqs-ubuntu.sh @@ -6,6 +6,7 @@ OPT="/opt" BASE_PKGS="gawk make git arduino-core curl" SITL_PKGS="g++ python-pip python-matplotlib python-serial python-wxgtk2.8 python-scipy python-opencv python-numpy python-pyparsing ccache" +AVR_PKGS="gcc-avr binutils-avr avr-libc" PYTHON_PKGS="pymavlink MAVProxy droneapi" PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \ autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \ @@ -15,9 +16,9 @@ ASSUME_YES=false # GNU Tools for ARM Embedded Processors # (see https://launchpad.net/gcc-arm-embedded/) -ARM_ROOT="gcc-arm-none-eabi-4_8-2013q4" -ARM_TARBALL="$ARM_ROOT-20131204-linux.tar.bz2" -ARM_TARBALL_URL="https://launchpad.net/gcc-arm-embedded/4.8/4.8-2013-q4-major/+download/$ARM_TARBALL" +ARM_ROOT="gcc-arm-none-eabi-4_7-2014q2" +ARM_TARBALL="$ARM_ROOT-20140408-linux.tar.bz2" +ARM_TARBALL_URL="http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL" # Ardupilot Tools ARDUPILOT_TOOLS="ardupilot/Tools/autotest" @@ -57,7 +58,7 @@ sudo usermod -a -G dialout $USER $APT_GET remove modemmanager $APT_GET update -$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $UBUNTU64_PKGS +$APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $UBUNTU64_PKGS $AVR_PKGS sudo pip -q install $PYTHON_PKGS @@ -69,6 +70,10 @@ if [ ! -d PX4NuttX ]; then git clone https://github.com/diydrones/PX4NuttX.git fi +if [ ! -d uavcan ]; then + git clone https://github.com/diydrones/uavcan.git +fi + if [ ! -d VRNuttX ]; then git clone https://github.com/virtualrobotix/vrbrain_nuttx.git VRNuttX fi From f053af5312dbe1e6a6dfda2ee1000dea3eec083c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:22:45 +1100 Subject: [PATCH 221/254] Travis: only build copter in copter-3.2 branch --- Tools/scripts/build_all_travis.sh | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/Tools/scripts/build_all_travis.sh b/Tools/scripts/build_all_travis.sh index b8e2e7e503b..68daf012dc5 100755 --- a/Tools/scripts/build_all_travis.sh +++ b/Tools/scripts/build_all_travis.sh @@ -9,7 +9,7 @@ set -e set -x echo "Testing ArduPlane build" -pushd ArduPlane +pushd ArduCopter make configure for b in all apm2 sitl linux; do pwd @@ -18,7 +18,7 @@ for b in all apm2 sitl linux; do done popd -for d in ArduCopter APMrover2 ArduPlane AntennaTracker; do +for d in ArduCopter; do pushd $d make clean make sitl -j4 @@ -30,9 +30,4 @@ for d in ArduCopter APMrover2 ArduPlane AntennaTracker; do popd done -pushd Tools/Replay -make clean -make linux -j4 -popd - exit 0 From 8da5716ea723dffd110b5c0a0335c4ecbba8a6c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:25:23 +1100 Subject: [PATCH 222/254] Travis: don't build Linux for travis in copter-3.2 --- Tools/scripts/build_all_travis.sh | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Tools/scripts/build_all_travis.sh b/Tools/scripts/build_all_travis.sh index 68daf012dc5..64ac54a3d11 100755 --- a/Tools/scripts/build_all_travis.sh +++ b/Tools/scripts/build_all_travis.sh @@ -11,7 +11,7 @@ set -x echo "Testing ArduPlane build" pushd ArduCopter make configure -for b in all apm2 sitl linux; do +for b in all apm2 sitl; do pwd make clean make $b -j4 @@ -23,8 +23,6 @@ for d in ArduCopter; do make clean make sitl -j4 make clean - make linux -j4 - make clean make px4-cleandep make px4-v2 popd From 712b95bf8ef744435b7fdd605923b0b586503c35 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:41:38 +1100 Subject: [PATCH 223/254] AP_HAL: added micros64() method --- libraries/AP_HAL/Scheduler.h | 6 +++++ libraries/AP_HAL_AVR_SITL/Scheduler.cpp | 36 ++++++++++++++++--------- libraries/AP_HAL_AVR_SITL/Scheduler.h | 4 ++- libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 2 +- libraries/AP_HAL_Empty/Scheduler.cpp | 12 +++++++-- libraries/AP_HAL_Empty/Scheduler.h | 2 ++ libraries/AP_HAL_PX4/Scheduler.cpp | 22 ++++++++++----- libraries/AP_HAL_PX4/Scheduler.h | 2 ++ 8 files changed, 64 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index 8606a31cdbe..7238370da65 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -3,6 +3,7 @@ #define __AP_HAL_SCHEDULER_H__ #include "AP_HAL_Namespace.h" +#include "AP_HAL_Boards.h" #include #include @@ -14,6 +15,11 @@ class AP_HAL::Scheduler { virtual void delay(uint16_t ms) = 0; virtual uint32_t millis() = 0; virtual uint32_t micros() = 0; +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + // offer non-wrapping 64 bit versions on faster CPUs + virtual uint64_t millis64() = 0; + virtual uint64_t micros64() = 0; +#endif virtual void delay_microseconds(uint16_t us) = 0; virtual void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms) = 0; diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp index db0d1728249..5fa731f58a9 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_AVR_SITL/Scheduler.cpp @@ -70,52 +70,64 @@ double SITLScheduler::_cyg_sec() } #endif -uint32_t SITLScheduler::_micros() +uint64_t SITLScheduler::_micros64() { #ifdef __CYGWIN__ - return (uint32_t)(_cyg_sec() * 1.0e6); + return (uint64_t)(_cyg_sec() * 1.0e6); #else struct timeval tp; gettimeofday(&tp,NULL); - return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - (_sketch_start_time.tv_sec + (_sketch_start_time.tv_usec*1.0e-6))); + return ret; #endif } +uint64_t SITLScheduler::micros64() +{ + return _micros64(); +} + uint32_t SITLScheduler::micros() { - return _micros(); + return micros64() & 0xFFFFFFFF; } -uint32_t SITLScheduler::millis() +uint64_t SITLScheduler::millis64() { #ifdef __CYGWIN__ // 1000 ms in a second - return (uint32_t)(_cyg_sec() * 1000); + return (uint64_t)(_cyg_sec() * 1000); #else struct timeval tp; gettimeofday(&tp,NULL); - return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - (_sketch_start_time.tv_sec + (_sketch_start_time.tv_usec*1.0e-6))); + return ret; #endif } +uint32_t SITLScheduler::millis() +{ + return millis64() & 0xFFFFFFFF; +} + void SITLScheduler::delay_microseconds(uint16_t usec) { - uint32_t start = micros(); - while (micros() - start < usec) { - usleep(usec - (micros() - start)); + uint64_t start = micros64(); + while (micros64() - start < usec) { + usleep(usec - (micros64() - start)); } } void SITLScheduler::delay(uint16_t ms) { - uint32_t start = micros(); + uint64_t start = micros64(); while (ms > 0) { - while ((micros() - start) >= 1000) { + while ((micros64() - start) >= 1000) { ms--; if (ms == 0) break; start += 1000; diff --git a/libraries/AP_HAL_AVR_SITL/Scheduler.h b/libraries/AP_HAL_AVR_SITL/Scheduler.h index bd6e33aba9a..8719e723e02 100644 --- a/libraries/AP_HAL_AVR_SITL/Scheduler.h +++ b/libraries/AP_HAL_AVR_SITL/Scheduler.h @@ -19,6 +19,8 @@ class AVR_SITL::SITLScheduler : public AP_HAL::Scheduler { void delay(uint16_t ms); uint32_t millis(); uint32_t micros(); + uint64_t millis64(); + uint64_t micros64(); void delay_microseconds(uint16_t us); void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); @@ -43,7 +45,7 @@ class AVR_SITL::SITLScheduler : public AP_HAL::Scheduler { void sitl_end_atomic(); // callable from interrupt handler - static uint32_t _micros(); + static uint64_t _micros64(); static void timer_event() { _run_timer_procs(true); _run_io_procs(true); } private: diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index 06714a84c15..6792b00540d 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -52,7 +52,7 @@ float SITL_State::_gyro_drift(void) return 0; } double period = _sitl->drift_time * 2; - double minutes = fmod(_scheduler->_micros() / 60.0e6, period); + double minutes = fmod(_scheduler->micros64() / 60.0e6, period); if (minutes < period/2) { return minutes * ToRad(_sitl->drift_speed); } diff --git a/libraries/AP_HAL_Empty/Scheduler.cpp b/libraries/AP_HAL_Empty/Scheduler.cpp index f2a4e16f56a..c12026c4883 100644 --- a/libraries/AP_HAL_Empty/Scheduler.cpp +++ b/libraries/AP_HAL_Empty/Scheduler.cpp @@ -14,14 +14,22 @@ void EmptyScheduler::init(void* machtnichts) void EmptyScheduler::delay(uint16_t ms) {} -uint32_t EmptyScheduler::millis() { +uint64_t EmptyScheduler::millis64() { return 10000; } -uint32_t EmptyScheduler::micros() { +uint64_t EmptyScheduler::micros64() { return 200000; } +uint32_t EmptyScheduler::millis() { + return millis64(); +} + +uint32_t EmptyScheduler::micros() { + return micros64(); +} + void EmptyScheduler::delay_microseconds(uint16_t us) {} diff --git a/libraries/AP_HAL_Empty/Scheduler.h b/libraries/AP_HAL_Empty/Scheduler.h index 75df768dc83..2afcbf460a1 100644 --- a/libraries/AP_HAL_Empty/Scheduler.h +++ b/libraries/AP_HAL_Empty/Scheduler.h @@ -11,6 +11,8 @@ class Empty::EmptyScheduler : public AP_HAL::Scheduler { void delay(uint16_t ms); uint32_t millis(); uint32_t micros(); + uint64_t millis64(); + uint64_t micros64(); void delay_microseconds(uint16_t us); void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index b097ba04690..856d684ef48 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -76,6 +76,16 @@ void PX4Scheduler::init(void *unused) pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_io_thread, this); } +uint64_t PX4Scheduler::micros64() +{ + return hrt_absolute_time(); +} + +uint64_t PX4Scheduler::millis64() +{ + return micros64() / 1000; +} + uint32_t PX4Scheduler::micros() { return (uint32_t)(hrt_absolute_time() - _sketch_start_time); @@ -83,7 +93,7 @@ uint32_t PX4Scheduler::micros() uint32_t PX4Scheduler::millis() { - return hrt_absolute_time() / 1000; + return millis64() & 0xFFFFFFFF; } /** @@ -106,9 +116,9 @@ void PX4Scheduler::delay_microseconds(uint16_t usec) perf_end(_perf_delay); return; } - uint32_t start = micros(); - uint32_t dt; - while ((dt=(micros() - start)) < usec) { + uint64_t start = micros64(); + uint64_t dt; + while ((dt=(micros64() - start)) < usec) { up_udelay(usec - dt); } perf_end(_perf_delay); @@ -121,9 +131,9 @@ void PX4Scheduler::delay(uint16_t ms) return; } perf_begin(_perf_delay); - uint64_t start = hrt_absolute_time(); + uint64_t start = micros64(); - while ((hrt_absolute_time() - start)/1000 < ms && + while ((micros64() - start)/1000 < ms && !_px4_thread_should_exit) { delay_microseconds_semaphore(1000); if (_min_delay_cb_ms <= ms) { diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index b88b997dd15..891eeb34567 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -29,6 +29,8 @@ class PX4::PX4Scheduler : public AP_HAL::Scheduler { void delay(uint16_t ms); uint32_t millis(); uint32_t micros(); + uint64_t millis64(); + uint64_t micros64(); void delay_microseconds(uint16_t us); void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); void register_timer_process(AP_HAL::MemberProc); From e7dcb43407b0d699100f655fb93e1ab239115c2a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:41:51 +1100 Subject: [PATCH 224/254] PX4: update to new PX4 config --- mk/PX4/config_px4fmu-v1_APM.mk | 78 +--------------------------------- mk/PX4/config_px4fmu-v2_APM.mk | 77 +-------------------------------- 2 files changed, 3 insertions(+), 152 deletions(-) diff --git a/mk/PX4/config_px4fmu-v1_APM.mk b/mk/PX4/config_px4fmu-v1_APM.mk index 36e535803f8..6f17f693a29 100644 --- a/mk/PX4/config_px4fmu-v1_APM.mk +++ b/mk/PX4/config_px4fmu-v1_APM.mk @@ -1,83 +1,7 @@ # # Makefile for the px4fmu-v1_APM configuration # +include $(SKETCHBOOK)/mk/PX4/px4_common.mk -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(SKETCHBOOK)/mk/PX4/ROMFS - -MODULES += $(APM_MODULE_DIR) - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/rgbled -MODULES += drivers/l3gd20 -# MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/ll40ls -MODULES += drivers/gps -#MODULES += drivers/hil -#MODULES += drivers/hott_telemetry -MODULES += drivers/blinkm -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += drivers/mkblctrl -#MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/mtd -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/perf -MODULES += systemcmds/pwm -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/nshterm -MODULES += systemcmds/auth - -# -# Libraries -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB -MODULES += lib/mathlib/math/filter -MODULES += modules/libtomfastmath -MODULES += modules/libtomcrypt -MODULES += lib/conversion - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) diff --git a/mk/PX4/config_px4fmu-v2_APM.mk b/mk/PX4/config_px4fmu-v2_APM.mk index 494b456ac6c..489562eb8a4 100644 --- a/mk/PX4/config_px4fmu-v2_APM.mk +++ b/mk/PX4/config_px4fmu-v2_APM.mk @@ -1,82 +1,9 @@ # # Makefile for the px4fmu-v2_APM configuration # +include $(SKETCHBOOK)/mk/PX4/px4_common.mk -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(SKETCHBOOK)/mk/PX4/ROMFS - -MODULES += $(APM_MODULE_DIR) - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4fmu -MODULES += drivers/px4io -MODULES += drivers/boards/px4fmu-v2 -MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/ll40ls -MODULES += drivers/gps -MODULES += drivers/hil -#MODULES += drivers/hott_telemetry -MODULES += drivers/blinkm -#MODULES += modules/sensors -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += drivers/mkblctrl - -# -# System commands -# -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/mixer -MODULES += systemcmds/perf -MODULES += systemcmds/pwm -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/nshterm -MODULES += systemcmds/auth -MODULES += systemcmds/mtd - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB -MODULES += lib/mathlib/math/filter -MODULES += modules/libtomfastmath -MODULES += modules/libtomcrypt -MODULES += lib/conversion - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef +MODULES += drivers/boards/px4fmu-v2 -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) From 3761870544bf818d4f06720092cf2f7b34c29e4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 13:42:06 +1100 Subject: [PATCH 225/254] build: update to build rules from master --- mk/board_avr.mk | 3 +++ mk/board_flymaple.mk | 3 +++ mk/board_native.mk | 3 +++ 3 files changed, 9 insertions(+) diff --git a/mk/board_avr.mk b/mk/board_avr.mk index f1d2357089a..e3cba572d60 100644 --- a/mk/board_avr.mk +++ b/mk/board_avr.mk @@ -124,6 +124,7 @@ endif # The ELF file SKETCHELF = $(BUILDROOT)/$(SKETCH).elf +BUILDELF = $(notdir $(SKETCHELF)) # HEX file SKETCHHEX = $(BUILDROOT)/$(SKETCH).hex @@ -174,6 +175,8 @@ jtag-program: $(SKETCHELF): $(SKETCHOBJS) $(LIBOBJS) $(RULEHDR) $(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS) + $(v)cp $(SKETCHELF) $(BUILDELF) + @echo "Firmware is in $(BUILDELF)" # Create the hex file $(SKETCHHEX): $(SKETCHELF) diff --git a/mk/board_flymaple.mk b/mk/board_flymaple.mk index e1c3cd4887d..2eb068e2a62 100644 --- a/mk/board_flymaple.mk +++ b/mk/board_flymaple.mk @@ -102,6 +102,7 @@ LIBOBJS := $(SKETCHLIBOBJS) $(COREOBJS) # The ELF file SKETCHELF = $(BUILDROOT)/$(SKETCH).elf +BUILDELF = $(notdir $(SKETCHELF)) # HEX file SKETCHHEX = $(BUILDROOT)/$(SKETCH).hex @@ -157,6 +158,8 @@ jtag-program: $(SKETCHELF): $(SKETCHOBJS) $(LIBOBJS) $(RULEHDR) $(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS) + $(v)cp $(SKETCHELF) . + @echo "Firmware is in $(BUILDELF)" # Create the hex file $(SKETCHHEX): $(SKETCHELF) diff --git a/mk/board_native.mk b/mk/board_native.mk index 9f16057eb93..9407f798e84 100644 --- a/mk/board_native.mk +++ b/mk/board_native.mk @@ -58,6 +58,7 @@ LIBOBJS := $(SKETCHLIBOBJS) # The ELF file SKETCHELF = $(BUILDROOT)/$(SKETCH).elf +BUILDELF = $(notdir $(SKETCHELF)) # HEX file SKETCHHEX = $(BUILDROOT)/$(SKETCH).hex @@ -95,6 +96,8 @@ $(SKETCHELF): $(SKETCHOBJS) $(LIBOBJS) @echo "Building $(SKETCHELF)" $(RULEHDR) $(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS) + $(v)cp $(SKETCHELF) . + @echo "Firmware is in $(BUILDELF)" # # Build sketch objects From b058f3fdce9b5f14bd53d70fcb545af172bf6a78 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 14:16:28 +1100 Subject: [PATCH 226/254] AP_InertialSensor: added missing files --- .../AP_InertialSensor_Backend.cpp | 72 ++ .../AP_InertialSensor_Backend.h | 87 ++ .../AP_InertialSensor_L3GD20.cpp | 635 +++++++++++++ .../AP_InertialSensor_L3GD20.h | 88 ++ .../AP_InertialSensor_LSM303D.cpp | 831 ++++++++++++++++++ .../AP_InertialSensor_LSM303D.h | 107 +++ 6 files changed, 1820 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Backend.h create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp new file mode 100644 index 00000000000..3ed5dd28f02 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -0,0 +1,72 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : + _imu(imu), + _product_id(AP_PRODUCT_ID_NONE) +{} + +/* + rotate gyro vector and add the gyro offset + */ +void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro) +{ + _imu._gyro[instance] = gyro; + _imu._gyro[instance].rotate(_imu._board_orientation); + _imu._gyro[instance] -= _imu._gyro_offset[instance]; + _imu._gyro_healthy[instance] = true; +} + +/* + rotate accel vector, scale and add the accel offset + */ +void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel) +{ + _imu._accel[instance] = accel; + _imu._accel[instance].rotate(_imu._board_orientation); + + const Vector3f &accel_scale = _imu._accel_scale[instance].get(); + _imu._accel[instance].x *= accel_scale.x; + _imu._accel[instance].y *= accel_scale.y; + _imu._accel[instance].z *= accel_scale.z; + _imu._accel[instance] -= _imu._accel_offset[instance]; + _imu._accel_healthy[instance] = true; +} + +// set accelerometer error_count +void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count) +{ + _imu._accel_error_count[instance] = error_count; +} + +// set gyro error_count +void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count) +{ + _imu._gyro_error_count[instance] = error_count; +} + +/* + return the default filter frequency in Hz for the sample rate + + This uses the sample_rate as a proxy for what type of vehicle it is + (ie. plane and rover run at 50Hz). Copters need a bit more filter + bandwidth + */ +uint8_t AP_InertialSensor_Backend::_default_filter(void) const +{ + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: + // on Rover and plane use a lower filter rate + return 15; + case AP_InertialSensor::RATE_100HZ: + return 30; + case AP_InertialSensor::RATE_200HZ: + return 30; + case AP_InertialSensor::RATE_400HZ: + return 30; + } + return 30; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h new file mode 100644 index 00000000000..acd6254ba10 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -0,0 +1,87 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + IMU driver backend class. Each supported gyro/accel sensor type + needs to have an object derived from this class. + + Note that drivers can implement just gyros or just accels, and can + also provide multiple gyro/accel instances. + */ +#ifndef __AP_INERTIALSENSOR_BACKEND_H__ +#define __AP_INERTIALSENSOR_BACKEND_H__ + +class AP_InertialSensor_Backend +{ +public: + AP_InertialSensor_Backend(AP_InertialSensor &imu); + + // we declare a virtual destructor so that drivers can + // override with a custom destructor if need be. + virtual ~AP_InertialSensor_Backend(void) {} + + /* + * Update the sensor data. Called by the frontend to transfer + * accumulated sensor readings to the frontend structure via the + * _rotate_and_offset_gyro() and _rotate_and_offset_accel() functions + */ + virtual bool update() = 0; + + /* + * return true if at least one accel sample is available in the backend + * since the last call to update() + */ + virtual bool accel_sample_available() = 0; + + /* + * return true if at least one gyro sample is available in the backend + * since the last call to update() + */ + virtual bool gyro_sample_available() = 0; + + /* + return the product ID + */ + int16_t product_id(void) const { return _product_id; } + +protected: + // access to frontend + AP_InertialSensor &_imu; + + // rotate gyro vector and offset + void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro); + + // rotate accel vector, scale and offset + void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel); + + // set accelerometer error_count + void _set_accel_error_count(uint8_t instance, uint32_t error_count); + + // set gyro error_count + void _set_gyro_error_count(uint8_t instance, uint32_t error_count); + + // backend should fill in its product ID from AP_PRODUCT_ID_* + int16_t _product_id; + + // return the default filter frequency in Hz for the sample rate + uint8_t _default_filter(void) const; + + // note that each backend is also expected to have a static detect() + // function which instantiates an instance of the backend sensor + // driver if the sensor is available +}; + +#endif // __AP_INERTIALSENSOR_BACKEND_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp new file mode 100644 index 00000000000..6a443398e3f --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp @@ -0,0 +1,635 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Coded by Víctor Mayoral Vilches using + * l3gd20.cpp from the PX4 Development Team. + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#if NOT_YET + +#include "AP_InertialSensor_L3GD20.h" + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + #define L3GD20_DRDY_PIN 70 +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + #include "../AP_HAL_Linux/GPIO.h" + #define L3GD20_DRDY_PIN BBB_P8_34 // GYRO_DRDY + #endif +#endif + +/* L3GD20 definitions */ +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 +#define WHO_I_AM 0xD4 + +#define ADDR_CTRL_REG1 0x20 +#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ +/* keep lowpass low to avoid noise issues */ +#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ +#define RANGE_250DPS (0<<4) +#define RANGE_500DPS (1<<4) +#define RANGE_2000DPS (3<<4) + +#define ADDR_CTRL_REG5 0x24 +#define ADDR_REFERENCE 0x25 +#define ADDR_OUT_TEMP 0x26 +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2A +#define ADDR_OUT_Y_H 0x2B +#define ADDR_OUT_Z_L 0x2C +#define ADDR_OUT_Z_H 0x2D +#define ADDR_FIFO_CTRL_REG 0x2E +#define ADDR_FIFO_SRC_REG 0x2F +#define ADDR_INT1_CFG 0x30 +#define ADDR_INT1_SRC 0x31 +#define ADDR_INT1_TSH_XH 0x32 +#define ADDR_INT1_TSH_XL 0x33 +#define ADDR_INT1_TSH_YH 0x34 +#define ADDR_INT1_TSH_YL 0x35 +#define ADDR_INT1_TSH_ZH 0x36 +#define ADDR_INT1_TSH_ZL 0x37 +#define ADDR_INT1_DURATION 0x38 + +/* Internal configuration values */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define REG4_BDU (1<<7) +#define REG4_BLE (1<<6) +//#define REG4_SPI_3WIRE (1<<0) + +#define REG5_FIFO_ENABLE (1<<6) +#define REG5_REBOOT_MEMORY (1<<7) + +#define STATUS_ZYXOR (1<<7) +#define STATUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define FIFO_CTRL_BYPASS_MODE (0<<5) +#define FIFO_CTRL_FIFO_MODE (1<<5) +#define FIFO_CTRL_STREAM_MODE (1<<6) +#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) +#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) + +#define L3GD20_DEFAULT_RATE 760 +#define L3GD20_DEFAULT_RANGE_DPS 2000 +#define L3GD20_DEFAULT_FILTER_FREQ 30 + + +// const float AP_InertialSensor_L3GD20::_gyro_scale = (0.0174532f / 16.4f); + + +AP_InertialSensor_L3GD20::AP_InertialSensor_L3GD20() : + AP_InertialSensor(), + _drdy_pin(NULL), + _initialised(false), + _L3GD20_product_id(AP_PRODUCT_ID_NONE) +{ +} + +uint16_t AP_InertialSensor_L3GD20::_init_sensor( Sample_rate sample_rate ) +{ + if (_initialised) return _L3GD20_product_id; + _initialised = true; + + _spi = hal.spi->device(AP_HAL::SPIDevice_L3GD20); + _spi_sem = _spi->get_semaphore(); + +#ifdef L3GD20_DRDY_PIN + _drdy_pin = hal.gpio->channel(L3GD20_DRDY_PIN); + _drdy_pin->mode(HAL_GPIO_INPUT); +#endif + + hal.scheduler->suspend_timer_procs(); + + // Test WHOAMI + uint8_t whoami = _register_read(ADDR_WHO_AM_I); + if (whoami != WHO_I_AM) { + // TODO: we should probably accept multiple chip + // revisions. This is the one on the PXF + hal.console->printf("L3GD20: unexpected WHOAMI 0x%x\n", (unsigned)whoami); + hal.scheduler->panic(PSTR("L3GD20: bad WHOAMI")); + } + + uint8_t tries = 0; + do { + bool success = _hardware_init(sample_rate); + if (success) { + hal.scheduler->delay(5+2); + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore")); + } + if (_data_ready()) { + _spi_sem->give(); + break; + } else { + hal.console->println_P( + PSTR("L3GD20 startup failed: no data ready")); + } + _spi_sem->give(); + } + if (tries++ > 5) { + hal.scheduler->panic(PSTR("PANIC: failed to boot L3GD20 5 times")); + } + } while (1); + + hal.scheduler->resume_timer_procs(); + + + /* read the first lot of data. + * _read_data_transaction requires the spi semaphore to be taken by + * its caller. */ + _last_sample_time_micros = hal.scheduler->micros(); + hal.scheduler->delay(10); + if (_spi_sem->take(100)) { + _read_data_transaction(); + _spi_sem->give(); + } + + // start the timer process to read samples + hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3GD20::_poll_data)); + +#if L3GD20_DEBUG + _dump_registers(); +#endif + return _L3GD20_product_id; +} + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +bool AP_InertialSensor_L3GD20::wait_for_sample(uint16_t timeout_ms) +{ + if (_sample_available()) { + return true; + } + uint32_t start = hal.scheduler->millis(); + while ((hal.scheduler->millis() - start) < timeout_ms) { + hal.scheduler->delay_microseconds(100); + if (_sample_available()) { + return true; + } + } + return false; +} + +bool AP_InertialSensor_L3GD20::update( void ) +{ + // wait for at least 1 sample + if (!wait_for_sample(1000)) { + return false; + } + + // disable timer procs for mininum time + hal.scheduler->suspend_timer_procs(); + _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); + _num_samples = _sum_count; + _gyro_sum.zero(); + _sum_count = 0; + hal.scheduler->resume_timer_procs(); + + _gyro[0].rotate(_board_orientation); + _gyro[0] *= _gyro_scale / _num_samples; + _gyro[0] -= _gyro_offset[0]; + + // if (_last_filter_hz != _L3GD20_filter) { + // if (_spi_sem->take(10)) { + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + // _set_filter_register(_L3GD20_filter, 0); + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + // _error_count = 0; + // _spi_sem->give(); + // } + // } + + return true; +} + +/*================ HARDWARE FUNCTIONS ==================== */ + +/** + * Return true if the L3GD20 has new data available for reading. + * + * We use the data ready pin if it is available. Otherwise, read the + * status register. + */ +bool AP_InertialSensor_L3GD20::_data_ready() +{ + if (_drdy_pin) { + return _drdy_pin->read() != 0; + } + // TODO: read status register + return false; +} + +/** + * Timer process to poll for new data from the L3GD20. + */ +void AP_InertialSensor_L3GD20::_poll_data(void) +{ + if (hal.scheduler->in_timerprocess()) { + if (!_spi_sem->take_nonblocking()) { + /* + the semaphore being busy is an expected condition when the + mainline code is calling wait_for_sample() which will + grab the semaphore. We return now and rely on the mainline + code grabbing the latest sample. + */ + return; + } + if (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + /* Synchronous read - take semaphore */ + if (_spi_sem->take(10)) { + if (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + hal.scheduler->panic( + PSTR("PANIC: AP_InertialSensor_L3GD20::_poll_data " + "failed to take SPI semaphore synchronously")); + } + } +} + +void AP_InertialSensor_L3GD20::_read_data_transaction() { + + struct { + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report; + + /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); + raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; + _spi->transaction((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); + +#if L3GD20_USE_DRDY + if ((raw_report.status & 0xF) != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + hal.console->println_P( + PSTR("L3GD20: DRDY is not on in all axes, transfer error")); + return; + } +#endif + _gyro_sum.x += raw_report.x; + _gyro_sum.y += raw_report.y; + _gyro_sum.z -= raw_report.z; + _sum_count++; + + if (_sum_count == 0) { + // rollover - v unlikely + _gyro_sum.zero(); + } +} + +uint8_t AP_InertialSensor_L3GD20::_register_read( uint8_t reg ) +{ + uint8_t addr = reg | 0x80; // Set most significant bit + + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = addr; + tx[1] = 0; + _spi->transaction(tx, rx, 2); + + return rx[1]; +} + +void AP_InertialSensor_L3GD20::_register_write(uint8_t reg, uint8_t val) +{ + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = reg; + tx[1] = val; + _spi->transaction(tx, rx, 2); +} + +/* + useful when debugging SPI bus errors + */ +void AP_InertialSensor_L3GD20::_register_write_check(uint8_t reg, uint8_t val) +{ + uint8_t readed; + _register_write(reg, val); + readed = _register_read(reg); + if (readed != val){ + hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed); + } +#if L3GD20_DEBUG + hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed); +#endif +} + +/* + set the DLPF filter frequency. Assumes caller has taken semaphore + TODO needs to be changed according to L3GD20 needs + */ +// void AP_InertialSensor_L3GD20::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +// { +// uint8_t filter = default_filter; +// // choose filtering frequency +// switch (filter_hz) { +// case 5: +// filter = BITS_DLPF_CFG_5HZ; +// break; +// case 10: +// filter = BITS_DLPF_CFG_10HZ; +// break; +// case 20: +// filter = BITS_DLPF_CFG_20HZ; +// break; +// case 42: +// filter = BITS_DLPF_CFG_42HZ; +// break; +// case 98: +// filter = BITS_DLPF_CFG_98HZ; +// break; +// } + +// if (filter != 0) { +// _last_filter_hz = filter_hz; +// _register_write(MPUREG_CONFIG, filter); +// } +// } + + +void AP_InertialSensor_L3GD20::disable_i2c(void) +{ + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = _register_read(0x05); + _register_write(0x05, (0x20 | a)); + if (_register_read(0x05) == (a | 0x20)) { + return; + } + } + hal.scheduler->panic(PSTR("L3GD20: Unable to disable I2C")); +} + +uint8_t AP_InertialSensor_L3GD20::set_samplerate(uint16_t frequency) +{ + uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; + if (frequency == 0) + frequency = 760; + + /* use limits good for H or non-H models */ + if (frequency <= 100) { + // _current_rate = 95; + bits |= RATE_95HZ_LP_25HZ; + + } else if (frequency <= 200) { + // _current_rate = 190; + bits |= RATE_190HZ_LP_50HZ; + + } else if (frequency <= 400) { + // _current_rate = 380; + bits |= RATE_380HZ_LP_50HZ; + + } else if (frequency <= 800) { + // _current_rate = 760; + bits |= RATE_760HZ_LP_50HZ; + } else { + return -1; + } + _register_write(ADDR_CTRL_REG1, bits); + return 0; +} + +uint8_t AP_InertialSensor_L3GD20::set_range(uint8_t max_dps) +{ + uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; + float new_range; + + if (max_dps == 0) { + max_dps = 2000; + } + if (max_dps <= 250) { + new_range = 250; + bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; + + } else if (max_dps <= 500) { + new_range = 500; + bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; + + } else if (max_dps <= 2000) { + new_range = 2000; + bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; + + } else { + return -1; + } + + // _gyro_range_rad_s = new_range / 180.0f * M_PI_F; + // _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; + _gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; + _register_write(ADDR_CTRL_REG4, bits); + return 0; +} + +bool AP_InertialSensor_L3GD20::_hardware_init(Sample_rate sample_rate) +{ + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("L3GD20: Unable to get semaphore")); + } + + // initially run the bus at low speed + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + + // Chip reset + /* set default configuration */ + _register_write(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + hal.scheduler->delay(1); + _register_write(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + hal.scheduler->delay(1); + _register_write(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ + hal.scheduler->delay(1); + _register_write(ADDR_CTRL_REG4, REG4_BDU); + hal.scheduler->delay(1); + _register_write(ADDR_CTRL_REG5, 0); + hal.scheduler->delay(1); + + _register_write(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + hal.scheduler->delay(1); + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + _register_write(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + hal.scheduler->delay(1); + + set_samplerate(0); // 760Hz + hal.scheduler->delay(1); + set_range(L3GD20_DEFAULT_RANGE_DPS); + hal.scheduler->delay(1); + + // //TODO: Filtering + // uint8_t default_filter; + + // // sample rate and filtering + // // to minimise the effects of aliasing we choose a filter + // // that is less than half of the sample rate + // switch (sample_rate) { + // case RATE_50HZ: + // // this is used for plane and rover, where noise resistance is + // // more important than update rate. Tests on an aerobatic plane + // // show that 10Hz is fine, and makes it very noise resistant + // default_filter = BITS_DLPF_CFG_10HZ; + // _sample_shift = 2; + // break; + // case RATE_100HZ: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 1; + // break; + // case RATE_200HZ: + // default: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 0; + // break; + // } + // _set_filter_register(_L3GD20_filter, default_filter); + + // now that we have initialised, we set the SPI bus speed to high + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _spi_sem->give(); + + return true; +} + +// return the MPU6k gyro drift rate in radian/s/s +// note that this is much better than the oilpan gyros +float AP_InertialSensor_L3GD20::get_gyro_drift_rate(void) +{ + // 0.5 degrees/second/minute + return ToRad(0.5/60); +} + +// return true if a sample is available +bool AP_InertialSensor_L3GD20::_sample_available() +{ + _poll_data(); + // return (_sum_count >> _sample_shift) > 0; + return (_sum_count) > 0; +} + + +#if L3GD20_DEBUG +// dump all config registers - used for debug +void AP_InertialSensor_L3GD20::_dump_registers(void) +{ + hal.console->println_P(PSTR("L3GD20 registers")); + if (_spi_sem->take(100)) { + for (uint8_t reg=ADDR_WHO_AM_I; reg<=56; reg++) { // 0x38 = 56 + uint8_t v = _register_read(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (ADDR_WHO_AM_I-1)) % 16 == 0) { + hal.console->println(); + } + } + hal.console->println(); + _spi_sem->give(); + } +} +#endif + + +// get_delta_time returns the time period in seconds overwhich the sensor data was collected +float AP_InertialSensor_L3GD20::get_delta_time() const +{ + // the sensor runs at 200Hz + return 0.005 * _num_samples; +} +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h new file mode 100644 index 00000000000..ca7c5023397 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h @@ -0,0 +1,88 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_INERTIAL_SENSOR_L3GD20_H__ +#define __AP_INERTIAL_SENSOR_L3GD20_H__ + +#include +#include +#include +#include +#include "AP_InertialSensor.h" + +// enable debug to see a register dump on startup +#define L3GD20_DEBUG 0 + +class AP_InertialSensor_L3GD20 : public AP_InertialSensor +{ +public: + + AP_InertialSensor_L3GD20(); + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + float get_gyro_drift_rate(); + + // wait for a sample to be available, with timeout in milliseconds + bool wait_for_sample(uint16_t timeout_ms); + + // get_delta_time returns the time period in seconds overwhich the sensor data was collected + float get_delta_time() const; + + uint16_t error_count(void) const { return _error_count; } + bool healthy(void) const { return _error_count <= 4; } + bool get_gyro_health(uint8_t instance) const { return healthy(); } + bool get_accel_health(uint8_t instance) const { return healthy(); } + +protected: + uint16_t _init_sensor( Sample_rate sample_rate ); + +private: + AP_HAL::DigitalSource *_drdy_pin; + + bool _sample_available(); + void _read_data_transaction(); + bool _data_ready(); + void _poll_data(void); + uint8_t _register_read( uint8_t reg ); + void _register_write( uint8_t reg, uint8_t val ); + void _register_write_check(uint8_t reg, uint8_t val); + bool _hardware_init(Sample_rate sample_rate); + void disable_i2c(void); + uint8_t set_samplerate(uint16_t frequency); + uint8_t set_range(uint8_t max_dps); + + AP_HAL::SPIDeviceDriver *_spi; + AP_HAL::Semaphore *_spi_sem; + + uint16_t _num_samples; + float _gyro_scale; + + uint32_t _last_sample_time_micros; + + // ensure we can't initialise twice + bool _initialised; + int16_t _L3GD20_product_id; + + // how many hardware samples before we report a sample to the caller + uint8_t _sample_shift; + + // support for updating filter at runtime + uint8_t _last_filter_hz; + + void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + + uint16_t _error_count; + + // accumulation in timer - must be read with timer disabled + // the sum of the values since last read + Vector3l _gyro_sum; + volatile int16_t _sum_count; + +public: + +#if L3GD20_DEBUG + void _dump_registers(void); +#endif +}; + +#endif // __AP_INERTIAL_SENSOR_L3GD20_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp new file mode 100644 index 00000000000..b6c6b7d341c --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp @@ -0,0 +1,831 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#if NOT_YET + +/**************************************************************************** + * + * Coded by Víctor Mayoral Vilches using + * lsm3030d.cpp from the PX4 Development Team. + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "AP_InertialSensor_LSM303D.h" + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + #define LSM303D_DRDY_PIN 70 +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + #include "../AP_HAL_Linux/GPIO.h" + #define LSM303D_DRDY_X_PIN BBB_P8_8 // ACCEL DRDY + #define LSM303D_DRDY_M_PIN BBB_P8_10 // MAGNETOMETER DRDY + #endif +#endif + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e + +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG0 0x1F +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG6 0x25 +#define ADDR_CTRL_REG7 0x26 + +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + +#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) + +#define REG1_BDU_UPDATE (1<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) + +#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) + +#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) + +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) + +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) + + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + +/* default values for this device */ +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 + +#define LSM303D_ONE_G 9.80665f + + +AP_InertialSensor_LSM303D::AP_InertialSensor_LSM303D() : + AP_InertialSensor(), + _drdy_pin_x(NULL), + _drdy_pin_m(NULL), + _initialised(false), + _LSM303D_product_id(AP_PRODUCT_ID_NONE) +{ +} + +uint16_t AP_InertialSensor_LSM303D::_init_sensor( Sample_rate sample_rate ) +{ + if (_initialised) return _LSM303D_product_id; + _initialised = true; + + _spi = hal.spi->device(AP_HAL::SPIDevice_LSM303D); + _spi_sem = _spi->get_semaphore(); + +// This device has mag and accel +#ifdef LSM303D_DRDY_X_PIN + _drdy_pin_x = hal.gpio->channel(LSM303D_DRDY_X_PIN); + _drdy_pin_x->mode(HAL_GPIO_INPUT); +#endif + +#ifdef LSM303D_DRDY_M_PIN + _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN); + _drdy_pin_m->mode(HAL_GPIO_INPUT); +#endif + + hal.scheduler->suspend_timer_procs(); + + // Test WHOAMI + uint8_t whoami = _register_read(ADDR_WHO_AM_I); + if (whoami != WHO_I_AM) { + // TODO: we should probably accept multiple chip + // revisions. This is the one on the PXF + hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); + hal.scheduler->panic(PSTR("LSM303D: bad WHOAMI")); + } + + uint8_t tries = 0; + do { + bool success = _hardware_init(sample_rate); + if (success) { + hal.scheduler->delay(5+2); + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore")); + } + if (_data_ready()) { + _spi_sem->give(); + break; + } else { + hal.console->println_P( + PSTR("LSM303D startup failed: no data ready")); + } + _spi_sem->give(); + } + if (tries++ > 5) { + hal.scheduler->panic(PSTR("PANIC: failed to boot LSM303D 5 times")); + } + } while (1); + + hal.scheduler->resume_timer_procs(); + + + /* read the first lot of data. + * _read_data_transaction requires the spi semaphore to be taken by + * its caller. */ + _last_sample_time_micros = hal.scheduler->micros(); + hal.scheduler->delay(10); + if (_spi_sem->take(100)) { + _read_data_transaction(); + _spi_sem->give(); + } + + // start the timer process to read samples + hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_LSM303D::_poll_data)); + +#if LSM303D_DEBUG + _dump_registers(); +#endif + return _LSM303D_product_id; +} + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +bool AP_InertialSensor_LSM303D::wait_for_sample(uint16_t timeout_ms) +{ + if (_sample_available()) { + return true; + } + uint32_t start = hal.scheduler->millis(); + while ((hal.scheduler->millis() - start) < timeout_ms) { + hal.scheduler->delay_microseconds(100); + if (_sample_available()) { + return true; + } + } + return false; +} + +bool AP_InertialSensor_LSM303D::update( void ) +{ + // wait for at least 1 sample + if (!wait_for_sample(1000)) { + return false; + } + + // disable timer procs for mininum time + hal.scheduler->suspend_timer_procs(); + + _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); + // _mag[0] = Vector3f(_mag_sum.x, _mag_sum.y, _mag_sum.z); + + _num_samples = _sum_count; + _accel_sum.zero(); + _mag_sum.zero(); + _sum_count = 0; + hal.scheduler->resume_timer_procs(); + + _accel[0].rotate(_board_orientation); + // TODO change this for the corresponding value + // _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; + + // Vector3f accel_scale = _accel_scale[0].get(); + // _accel[0].x *= accel_scale.x; + // _accel[0].y *= accel_scale.y; + // _accel[0].z *= accel_scale.z; + // _accel[0] -= _accel_offset[0]; + + // TODO similarly put mag values in _mag and scale them + + // if (_last_filter_hz != _LSM303D_filter) { + // if (_spi_sem->take(10)) { + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + // _set_filter_register(_LSM303D_filter, 0); + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + // _error_count = 0; + // _spi_sem->give(); + // } + // } + + return true; +} + +/*================ HARDWARE FUNCTIONS ==================== */ + +/** + * Return true if the LSM303D has new data available for both the mag and the accels. + * + * We use the data ready pin if it is available. Otherwise, read the + * status register. + */ +bool AP_InertialSensor_LSM303D::_data_ready() +{ + if (_drdy_pin_m && _drdy_pin_x) { + return (_drdy_pin_m->read() && _drdy_pin_x->read()) != 0; + } + // TODO: read status register + return false; +} + +/** + * Timer process to poll for new data from the LSM303D. + */ +void AP_InertialSensor_LSM303D::_poll_data(void) +{ + if (hal.scheduler->in_timerprocess()) { + if (!_spi_sem->take_nonblocking()) { + /* + the semaphore being busy is an expected condition when the + mainline code is calling wait_for_sample() which will + grab the semaphore. We return now and rely on the mainline + code grabbing the latest sample. + */ + return; + } + if (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + /* Synchronous read - take semaphore */ + if (_spi_sem->take(10)) { + if (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + hal.scheduler->panic( + PSTR("PANIC: AP_InertialSensor_LSM303D::_poll_data " + "failed to take SPI semaphore synchronously")); + } + } +} + +void AP_InertialSensor_LSM303D::_read_data_transaction_accel() +{ + + if (_register_read(ADDR_CTRL_REG1) != _reg1_expected) { + hal.console->println_P( + PSTR("LSM303D _read_data_transaction_accel: _reg1_expected unexpected")); + // reset(); + return; + } + + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; + + /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + _spi->transaction((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + _accel_sum.x += raw_accel_report.x; + _accel_sum.y += raw_accel_report.y; + _accel_sum.z += raw_accel_report.z; +} + +void AP_InertialSensor_LSM303D::_read_data_transaction_mag() { + if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) { + hal.console->println_P( + PSTR("LSM303D _read_data_transaction_accel: _reg7_expected unexpected")); + // reset(); + return; + } + + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; + + /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + _spi->transaction((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + _mag_sum.x = raw_mag_report.x; + _mag_sum.y = raw_mag_report.y; + _mag_sum.z = raw_mag_report.z; +} + +void AP_InertialSensor_LSM303D::_read_data_transaction() { + + _read_data_transaction_accel(); + _read_data_transaction_mag(); + _sum_count++; + + if (_sum_count == 0) { + // rollover - v unlikely + _accel_sum.zero(); + _mag_sum.zero(); + } +} + +uint8_t AP_InertialSensor_LSM303D::_register_read( uint8_t reg ) +{ + uint8_t addr = reg | 0x80; // Set most significant bit + + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = addr; + tx[1] = 0; + _spi->transaction(tx, rx, 2); + + return rx[1]; +} + +void AP_InertialSensor_LSM303D::_register_write(uint8_t reg, uint8_t val) +{ + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = reg; + tx[1] = val; + _spi->transaction(tx, rx, 2); +} + +/* + useful when debugging SPI bus errors + */ +void AP_InertialSensor_LSM303D::_register_write_check(uint8_t reg, uint8_t val) +{ + uint8_t readed; + _register_write(reg, val); + readed = _register_read(reg); + if (readed != val){ + hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed); + } +#if LSM303D_DEBUG + hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed); +#endif +} + +void AP_InertialSensor_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = _register_read(reg); + val &= ~clearbits; + val |= setbits; + _register_write(reg, val); +} + + +/* + set the DLPF filter frequency. Assumes caller has taken semaphore + TODO needs to be changed according to LSM303D needs + */ +// void AP_InertialSensor_LSM303D::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +// { +// uint8_t filter = default_filter; +// // choose filtering frequency +// switch (filter_hz) { +// case 5: +// filter = BITS_DLPF_CFG_5HZ; +// break; +// case 10: +// filter = BITS_DLPF_CFG_10HZ; +// break; +// case 20: +// filter = BITS_DLPF_CFG_20HZ; +// break; +// case 42: +// filter = BITS_DLPF_CFG_42HZ; +// break; +// case 98: +// filter = BITS_DLPF_CFG_98HZ; +// break; +// } + +// if (filter != 0) { +// _last_filter_hz = filter_hz; +// _register_write(MPUREG_CONFIG, filter); +// } +// } + +void AP_InertialSensor_LSM303D::disable_i2c(void) +{ + uint8_t a = _register_read(0x02); + _register_write(0x02, (0x10 | a)); + a = _register_read(0x02); + _register_write(0x02, (0xF7 & a)); + a = _register_read(0x15); + _register_write(0x15, (0x80 | a)); + a = _register_read(0x02); + _register_write(0x02, (0xE7 & a)); +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_range(uint8_t max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_scale_g_digit = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; + + } else if (max_g <= 4) { + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; + + } else if (max_g <= 6) { + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; + + } else if (max_g <= 8) { + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; + + } else if (max_g <= 16) { + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; + + } else { + return -1; + } + + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + _register_modify(ADDR_CTRL_REG2, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_samplerate(uint16_t frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + _accel_samplerate = 100; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + _accel_samplerate = 200; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + _accel_samplerate = 400; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + _accel_samplerate = 800; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + _accel_samplerate = 1600; + + } else { + return -1; + } + + _register_modify(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_onchip_lowpass_filter_bandwidth(uint8_t bandwidth) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_onchip_filter_bandwith = 50; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_onchip_filter_bandwith = 194; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_onchip_filter_bandwith = 362; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_onchip_filter_bandwith = 773; + + } else { + return -1; + } + + _register_modify(ADDR_CTRL_REG2, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::mag_set_range(uint8_t max_ga) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG6_FULL_SCALE_BITS_M; + float new_scale_ga_digit = 0.0f; + + if (max_ga == 0) + max_ga = 12; + + if (max_ga <= 2) { + _mag_range_ga = 2; + setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; + + } else if (max_ga <= 4) { + _mag_range_ga = 4; + setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; + + } else if (max_ga <= 8) { + _mag_range_ga = 8; + setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; + + } else if (max_ga <= 12) { + _mag_range_ga = 12; + setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; + + } else { + return -1; + } + + _mag_range_scale = new_scale_ga_digit; + _register_modify(ADDR_CTRL_REG6, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::mag_set_samplerate(uint16_t frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG5_RATE_BITS_M; + + if (frequency == 0) + frequency = 100; + + if (frequency <= 25) { + setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; + + } else if (frequency <= 50) { + setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; + + } else if (frequency <= 100) { + setbits |= REG5_RATE_100HZ_M; + _mag_samplerate = 100; + + } else { + return -1; + } + + _register_modify(ADDR_CTRL_REG5, clearbits, setbits); + return 0; +} + +bool AP_InertialSensor_LSM303D::_hardware_init(Sample_rate sample_rate) +{ + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore")); + } + + // initially run the bus at low speed + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + + + /* enable accel*/ + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + _register_write(ADDR_CTRL_REG1, _reg1_expected); + + /* enable mag */ + _reg7_expected = REG7_CONT_MODE_M; + _register_write(ADDR_CTRL_REG7, _reg7_expected); + _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + _register_write(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + _register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + + // Hardware filtering + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + + // TODO: Software filtering + // accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + + // uint8_t default_filter; + + // // sample rate and filtering + // // to minimise the effects of aliasing we choose a filter + // // that is less than half of the sample rate + // switch (sample_rate) { + // case RATE_50HZ: + // // this is used for plane and rover, where noise resistance is + // // more important than update rate. Tests on an aerobatic plane + // // show that 10Hz is fine, and makes it very noise resistant + // default_filter = BITS_DLPF_CFG_10HZ; + // _sample_shift = 2; + // break; + // case RATE_100HZ: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 1; + // break; + // case RATE_200HZ: + // default: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 0; + // break; + // } + // _set_filter_register(_LSM303D_filter, default_filter); + + // now that we have initialised, we set the SPI bus speed to high + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _spi_sem->give(); + + return true; +} + +// return true if a sample is available +bool AP_InertialSensor_LSM303D::_sample_available() +{ + _poll_data(); + // return (_sum_count >> _sample_shift) > 0; + return (_sum_count) > 0; +} + + +// TODO fix dump registers +#if LSM303D_DEBUG +// dump all config registers - used for debug +void AP_InertialSensor_LSM303D::_dump_registers(void) +{ + hal.console->println_P(PSTR("LSM303D registers")); + if (_spi_sem->take(100)) { + for (uint8_t reg=ADDR_WHO_AM_I; reg<=56; reg++) { // 0x38 = 56 + uint8_t v = _register_read(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (ADDR_WHO_AM_I-1)) % 16 == 0) { + hal.console->println(); + } + } + hal.console->println(); + _spi_sem->give(); + } +} +#endif + + +// get_delta_time returns the time period in seconds overwhich the sensor data was collected +float AP_InertialSensor_LSM303D::get_delta_time() const +{ + // the sensor runs at 200Hz + return 0.005 * _num_samples; +} +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h new file mode 100644 index 00000000000..a2c6fa2d10c --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h @@ -0,0 +1,107 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_INERTIAL_SENSOR_LSM303D_H__ +#define __AP_INERTIAL_SENSOR_LSM303D_H__ + +#include +#include +#include +#include +#include "AP_InertialSensor.h" + +// enable debug to see a register dump on startup +#define LSM303D_DEBUG 0 + +class AP_InertialSensor_LSM303D: public AP_InertialSensor +{ +public: + + AP_InertialSensor_LSM303D(); + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + + // wait for a sample to be available, with timeout in milliseconds + bool wait_for_sample(uint16_t timeout_ms); + + // get_delta_time returns the time period in seconds overwhich the sensor data was collected + float get_delta_time() const; + + uint16_t error_count(void) const { return _error_count; } + bool healthy(void) const { return _error_count <= 4; } + bool get_accel_health(uint8_t instance) const { return healthy(); } + +protected: + uint16_t _init_sensor( Sample_rate sample_rate ); + +private: + AP_HAL::DigitalSource *_drdy_pin_x; + AP_HAL::DigitalSource *_drdy_pin_m; + uint8_t _accel_range_m_s2; + float _accel_range_scale; + uint8_t _accel_samplerate; + uint8_t _accel_onchip_filter_bandwith; + uint8_t _mag_range_ga; + float _mag_range_scale; + uint8_t _mag_samplerate; + + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg1_expected; + uint8_t _reg7_expected; + + bool _sample_available(); + void _read_data_transaction(); + void _read_data_transaction_accel(); + void _read_data_transaction_mag(); + bool _data_ready(); + void _poll_data(void); + uint8_t _register_read( uint8_t reg ); + void _register_write( uint8_t reg, uint8_t val ); + void _register_write_check(uint8_t reg, uint8_t val); + void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits); + bool _hardware_init(Sample_rate sample_rate); + void disable_i2c(void); + uint8_t accel_set_range(uint8_t max_g); + uint8_t accel_set_samplerate(uint16_t frequency); + uint8_t accel_set_onchip_lowpass_filter_bandwidth(uint8_t bandwidth); + uint8_t mag_set_range(uint8_t max_ga); + uint8_t mag_set_samplerate(uint16_t frequency); + + AP_HAL::SPIDeviceDriver *_spi; + AP_HAL::Semaphore *_spi_sem; + + uint16_t _num_samples; + float _accel_scale; + float _mag_scale; + + uint32_t _last_sample_time_micros; + + // ensure we can't initialise twice + bool _initialised; + int16_t _LSM303D_product_id; + + // how many hardware samples before we report a sample to the caller + uint8_t _sample_shift; + + // support for updating filter at runtime + uint8_t _last_filter_hz; + + void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + + uint16_t _error_count; + + // accumulation in timer - must be read with timer disabled + // the sum of the values since last read + Vector3l _accel_sum; + Vector3l _mag_sum; + volatile int16_t _sum_count; + +public: + +#if LSM303D_DEBUG + void _dump_registers(void); +#endif +}; + +#endif // __AP_INERTIAL_SENSOR_LSM303D_H__ From 18b6c013dd48df1822a2e6ef99b1f2527c7a382c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Jan 2015 15:18:50 +1100 Subject: [PATCH 227/254] PX4: added px4_common.mk --- mk/PX4/px4_common.mk | 93 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 mk/PX4/px4_common.mk diff --git a/mk/PX4/px4_common.mk b/mk/PX4/px4_common.mk new file mode 100644 index 00000000000..9ec766ca137 --- /dev/null +++ b/mk/PX4/px4_common.mk @@ -0,0 +1,93 @@ +# +# common makefile elements for all PX4 boards +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(SKETCHBOOK)/mk/PX4/ROMFS +MODULES += $(APM_MODULE_DIR) + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/px4flow +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/ll40ls +#MODULES += drivers/gps +#MODULES += drivers/hil +#MODULES += drivers/hott_telemetry +#MODULES += drivers/blinkm +#MODULES += modules/sensors +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += drivers/mkblctrl +MODULES += drivers/batt_smbus + +# +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/perf +MODULES += systemcmds/pwm +MODULES += systemcmds/reboot +MODULES += systemcmds/top +#MODULES += systemcmds/tests +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +ifneq ($(wildcard $(PX4_ROOT)/src/systemcmds/reflect),) +MODULES += systemcmds/reflect +endif +ifneq ($(wildcard $(PX4_ROOT)/src/systemcmds/motor_test),) +MODULES += systemcmds/motor_test +endif + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB +MODULES += lib/mathlib/math/filter + +# Note: auth disabled to keep us under 1MB flash because of STM32 bug +#ifneq ($(wildcard $(PX4_ROOT)/src/systemcmds/auth),) +#MODULES += systemcmds/auth +#endif +#ifneq ($(wildcard $(PX4_ROOT)/src/modules/libtomfastmath),) +#MODULES += modules/libtomfastmath +#MODULES += modules/libtomcrypt +#endif + +MODULES += lib/conversion + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) From 113475b15862d9460269a51c273ccdc418bf9bf5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Jan 2015 08:41:54 +1100 Subject: [PATCH 228/254] autotest: use small INS offsets so INS is recognised as being calibrated --- Tools/autotest/ArduPlane.parm | 7 +++++++ Tools/autotest/Rover.parm | 7 +++++++ Tools/autotest/copter_AVC2013_params.parm | 7 +++++++ Tools/autotest/copter_params.parm | 7 +++++++ 4 files changed, 28 insertions(+) diff --git a/Tools/autotest/ArduPlane.parm b/Tools/autotest/ArduPlane.parm index 9c5575e7f66..bfa88d15d0d 100644 --- a/Tools/autotest/ArduPlane.parm +++ b/Tools/autotest/ArduPlane.parm @@ -65,3 +65,10 @@ ACRO_LOCKING 1 ELEVON_OUTPUT 0 VTAIL_OUTPUT 0 SKIP_GYRO_CAL 1 +# we need small INS_ACC offsets so INS is recognised as being calibrated +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 diff --git a/Tools/autotest/Rover.parm b/Tools/autotest/Rover.parm index 9e0628b132f..cd86b5eff92 100644 --- a/Tools/autotest/Rover.parm +++ b/Tools/autotest/Rover.parm @@ -18,3 +18,10 @@ MODE6 0 STEER2SRV_P 1.8 SIM_GPS_DELAY 2 NAVL1_PERIOD 6 +# we need small INS_ACC offsets so INS is recognised as being calibrated +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 diff --git a/Tools/autotest/copter_AVC2013_params.parm b/Tools/autotest/copter_AVC2013_params.parm index 32a9ee62d53..3c4c7c0f7bb 100644 --- a/Tools/autotest/copter_AVC2013_params.parm +++ b/Tools/autotest/copter_AVC2013_params.parm @@ -48,6 +48,13 @@ SIM_WIND_SPD 0 SIM_WIND_TURB 0 SIM_BARO_RND 0 SIM_MAG_RND 0 +# we need small INS_ACC offsets so INS is recognised as being calibrated +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 # flightmodes # switch 1 Circle # switch 2 LAND diff --git a/Tools/autotest/copter_params.parm b/Tools/autotest/copter_params.parm index 5b062baeb78..710b72a4cd4 100644 --- a/Tools/autotest/copter_params.parm +++ b/Tools/autotest/copter_params.parm @@ -46,6 +46,13 @@ SIM_WIND_SPD 0 SIM_WIND_TURB 0 SIM_BARO_RND 0 SIM_MAG_RND 0 +# we need small INS_ACC offsets so INS is recognised as being calibrated +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 # flightmodes # switch 1 Circle # switch 2 LAND From c0f5b548e02bfa6fef6e07ab177116b74bca2287 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Jan 2015 21:17:20 +0900 Subject: [PATCH 229/254] Copter: AC3.2.1-rc1 Release Notes --- ArduCopter/ReleaseNotes.txt | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 5b7beac9c96..ffbbae36e2a 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,21 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2.1-rc1 08-Jan-2014 +Changes from 3.2 +1) Enhancements: + a) reduced twitch when passing Spline waypoints + b) Faster disarm after landing in Auto, Land, RTL +2) Safety Features: + a) Add desired descent rate check to reduce chance of false-positive on landing check + b) improved MPU6k health monitoring and re-configuration in case of in-flight failure + c) Rally point distance check reduced to 300m (reduces chance of RTL to far away forgotten Rally point) + d) auto-disarm if vehicle is landed for 15seconds even in Auto, Guided, RTL, Circle + e) fence breach while vehicle is landed causes vehicle to disarm (previously did RTL) +3) Bug Fixes: + a) Check flight mode even when arming from GCS (previously it was possible to arm in RTL mode if arming was initiated from GCS) + b) Send vehicle target destination in RTL, Guided (allows GCS to show where vehicle is flying to in these modes) + c) PosHold wind compensation fix +------------------------------------------------------------------ ArduCopter 3.2 07-Nov2014 / 3.2-rc14 31-Oct-2014 Changes from 3.2-rc13 1) Safety Features: From 688088829beb1f7909d8519ce3d08b3eb8e5fd93 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Jan 2015 21:17:53 +0900 Subject: [PATCH 230/254] Copter: update version to AC3.2.1-rc1 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 54f0adacaa4..30499f90be6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2" +#define THISFIRMWARE "ArduCopter V3.2.1-rc1" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 43762c9fa4d2459fe42a1aa9041eed6ec7de1229 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 10:45:50 +0900 Subject: [PATCH 231/254] Copter: increase GPS_HDOP_GOOD default to 2.3 --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5c2496ab3d6..1f2440de451 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -302,7 +302,7 @@ # define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS #endif #ifndef GPS_HDOP_GOOD_DEFAULT - # define GPS_HDOP_GOOD_DEFAULT 200 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled + # define GPS_HDOP_GOOD_DEFAULT 230 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled #endif // GCS failsafe From f1207e019de3107de525bf39f0312d93c86a304f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Jan 2015 04:58:34 +0900 Subject: [PATCH 232/254] Copter: update AC3.2.1-rc1 Release Notes --- ArduCopter/ReleaseNotes.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index ffbbae36e2a..11fa04bb73b 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -5,6 +5,7 @@ Changes from 3.2 1) Enhancements: a) reduced twitch when passing Spline waypoints b) Faster disarm after landing in Auto, Land, RTL + c) Pixhawk LED turns green before arming only after GPS HDOP falls below 2.3 (only in flight modes requiring GPS) 2) Safety Features: a) Add desired descent rate check to reduce chance of false-positive on landing check b) improved MPU6k health monitoring and re-configuration in case of in-flight failure From 343dd1677993ecd5b8ae1433f8bcf1de6eb25317 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 10 Jan 2015 22:29:46 +0900 Subject: [PATCH 233/254] Copter: fix typo in AC3.2.1 Release Notes --- ArduCopter/ReleaseNotes.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 11fa04bb73b..58ce3e91535 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,6 @@ ArduCopter Release Notes: ------------------------------------------------------------------ -ArduCopter 3.2.1-rc1 08-Jan-2014 +ArduCopter 3.2.1-rc1 08-Jan-2015 Changes from 3.2 1) Enhancements: a) reduced twitch when passing Spline waypoints From 6b0e636abf92fa940211f14f4f2013524c5c023d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 22 Nov 2014 14:09:16 +0900 Subject: [PATCH 234/254] AP_Math: add yaw 293, pitch 68, roll 180 rotation --- libraries/AP_Math/rotations.h | 3 ++- libraries/AP_Math/vector3.cpp | 9 +++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 152e8e68c29..226b9d1419d 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -64,11 +64,12 @@ enum Rotation { ROTATION_ROLL_270_PITCH_270 = 35, ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, ROTATION_ROLL_90_YAW_270 = 37, + ROTATION_YAW_293_PITCH_68_ROLL_180 = 38, ROTATION_MAX }; /* Here are the same values in a form sutable for a @Values attribute in auto documentation: -@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270 +@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180 */ diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index b08a03dd30a..28fe757982b 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -221,6 +221,15 @@ void Vector3::rotate(enum Rotation rotation) tmp = x; x = y; y = -tmp; return; } + case ROTATION_YAW_293_PITCH_68_ROLL_180: { + float tmp = x; + float tmpy = y; + float tmpz = z; + x = 0.1430389f * tmp -0.9184465f * tmpy -0.3687762f * tmpz; + y = -0.3321327f * tmp -0.3955452f * tmpy +0.8562895f * tmpz; + z = -0.9323238f * tmp -0.00000003f * tmpy -0.3616245f * tmpz; + return; + } } } From 4b5411e86a7b9328b1247690fbb831df2fe9a0a9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 22 Nov 2014 14:10:00 +0900 Subject: [PATCH 235/254] AP_Math: add new rotation to example rotation sketch --- .../AP_Math/examples/rotations/rotations.pde | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 017e0ea6c0f..2327bd9a518 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -37,11 +37,10 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static void print_vector(Vector3f &v) { - hal.console->printf("[%.2f %.2f %.2f]\n", + hal.console->printf("[%.4f %.4f %.4f]\n", v.x, v.y, v.z); } - // test rotation method accuracy static void test_rotation_accuracy(void) { @@ -94,10 +93,16 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya diff = (v2 - v1); if (diff.length() > accuracy) { - hal.console->printf("euler test %u incorrect\n", (unsigned)rotation); - print_vector(v); + hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n", + (unsigned)rotation, + (int)yaw, + (int)roll, + (int)pitch); + hal.console->printf("fast rotated: "); print_vector(v1); + hal.console->printf("slow rotated: "); print_vector(v2); + hal.console->printf("\n"); } } @@ -141,7 +146,8 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_180_PITCH_270,180,270, 0); test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0); test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90); - test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); + test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); + test_euler(ROTATION_YAW_293_PITCH_68_ROLL_180,180,68.8,293.3); } static bool have_rotation(const Matrix3f &m) From eb9b2bf4e9f1171ff99ba0290e17a23f4f246e15 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Nov 2014 08:17:15 +1100 Subject: [PATCH 236/254] AP_Math: fixed build warning --- libraries/AP_Math/vector3.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 28fe757982b..8ec1634b0d1 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -222,12 +222,12 @@ void Vector3::rotate(enum Rotation rotation) return; } case ROTATION_YAW_293_PITCH_68_ROLL_180: { - float tmp = x; + float tmpx = x; float tmpy = y; float tmpz = z; - x = 0.1430389f * tmp -0.9184465f * tmpy -0.3687762f * tmpz; - y = -0.3321327f * tmp -0.3955452f * tmpy +0.8562895f * tmpz; - z = -0.9323238f * tmp -0.00000003f * tmpy -0.3616245f * tmpz; + x = 0.1430389f * tmpx -0.9184465f * tmpy -0.3687762f * tmpz; + y = -0.3321327f * tmpx -0.3955452f * tmpy +0.8562895f * tmpz; + z = -0.9323238f * tmpx -0.00000003f * tmpy -0.3616245f * tmpz; return; } } From 79975b130959eb014765846ac6dfa97b53813b45 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 31 Dec 2014 15:20:18 -0800 Subject: [PATCH 237/254] AP_Math: change ROTATION_YAW_293_PITCH_68_ROLL_180 to ROLL_90 --- libraries/AP_Math/examples/rotations/rotations.pde | 2 +- libraries/AP_Math/rotations.h | 2 +- libraries/AP_Math/vector3.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 2327bd9a518..9473c815bd9 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -147,7 +147,7 @@ static void test_eulers(void) test_euler(ROTATION_ROLL_270_PITCH_270,270,270, 0); test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90); test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270); - test_euler(ROTATION_YAW_293_PITCH_68_ROLL_180,180,68.8,293.3); + test_euler(ROTATION_YAW_293_PITCH_68_ROLL_90,90,68.8,293.3); } static bool have_rotation(const Matrix3f &m) diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 226b9d1419d..404f31dc5c6 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -64,7 +64,7 @@ enum Rotation { ROTATION_ROLL_270_PITCH_270 = 35, ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, ROTATION_ROLL_90_YAW_270 = 37, - ROTATION_YAW_293_PITCH_68_ROLL_180 = 38, + ROTATION_YAW_293_PITCH_68_ROLL_90 = 38, ROTATION_MAX }; /* diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 8ec1634b0d1..ae5399df326 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -221,13 +221,13 @@ void Vector3::rotate(enum Rotation rotation) tmp = x; x = y; y = -tmp; return; } - case ROTATION_YAW_293_PITCH_68_ROLL_180: { + case ROTATION_YAW_293_PITCH_68_ROLL_90: { float tmpx = x; float tmpy = y; float tmpz = z; - x = 0.1430389f * tmpx -0.9184465f * tmpy -0.3687762f * tmpz; - y = -0.3321327f * tmpx -0.3955452f * tmpy +0.8562895f * tmpz; - z = -0.9323238f * tmpx -0.00000003f * tmpy -0.3616245f * tmpz; + x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; + y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; + z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; return; } } From 2d82672893c31b6821982432507c202136a75976 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 12 Jan 2015 15:24:46 -0800 Subject: [PATCH 238/254] AC_PosControl: Fill _vel_desired.z for reporting --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0cd3a5d6d19..e3e3a96fcd0 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -122,6 +122,8 @@ void AC_PosControl::set_accel_z(float accel_cmss) void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) { float alt_change = alt_cm-_pos_target.z; + + _vel_desired.z = constrain_float(alt_change * dt, _speed_down_cms, _speed_up_cms); // adjust desired alt if motors have not hit their limits if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) { @@ -144,6 +146,8 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) { _pos_target.z += climb_rate_cms * dt; } + + _vel_desired.z = climb_rate_cms; } // get_alt_error - returns altitude error in cm From ca68a9a8a39d2c31a26089c72f3bf41aa5310893 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 12 Jan 2015 15:25:11 -0800 Subject: [PATCH 239/254] Copter: clean up land detector and modify to use desired velocity --- ArduCopter/control_land.pde | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 68c22e5068f..9547cc707ab 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -202,14 +202,13 @@ static bool land_complete_maybe() // called at 50hz static void update_land_detector() { - // detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate - if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && - (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX) && - motors.limit.throttle_lower && -#if FRAME_CONFIG != HELI_FRAME - (motors.get_throttle_out() < get_non_takeoff_throttle()) && -#endif - (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) { + bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX; + bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED; + bool motor_at_lower_limit = motors.limit.throttle_lower; + bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle(); + bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX; + + if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) { if (!ap.land_complete) { // increase counter until we hit the trigger then set land complete flag if( land_detector < LAND_DETECTOR_TRIGGER) { From 651a0182a743dc9885bba20279ca85999845c020 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Jan 2015 12:49:39 +0900 Subject: [PATCH 240/254] Copter: move land_detector to separate file --- ArduCopter/control_land.pde | 41 ---------------------------------- ArduCopter/land_detector.pde | 43 ++++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 41 deletions(-) create mode 100644 ArduCopter/land_detector.pde diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 9547cc707ab..c1c64efd2b5 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -1,7 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// counter to verify landings -static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed static bool land_with_gps; static uint32_t land_start_time; @@ -192,45 +190,6 @@ static float get_throttle_land() } } -// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing) -static bool land_complete_maybe() -{ - return (ap.land_complete || ap.land_complete_maybe); -} - -// update_land_detector - checks if we have landed and updates the ap.land_complete flag -// called at 50hz -static void update_land_detector() -{ - bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX; - bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED; - bool motor_at_lower_limit = motors.limit.throttle_lower; - bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle(); - bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX; - - if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) { - if (!ap.land_complete) { - // increase counter until we hit the trigger then set land complete flag - if( land_detector < LAND_DETECTOR_TRIGGER) { - land_detector++; - }else{ - set_land_complete(true); - land_detector = LAND_DETECTOR_TRIGGER; - } - } - } else { - // we've sensed movement up or down so reset land_detector - land_detector = 0; - // if throttle output is high then clear landing flag - if (motors.get_throttle_out() > get_non_takeoff_throttle()) { - set_land_complete(false); - } - } - - // set land maybe flag - set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER); -} - // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch // called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS // has no effect if we are not already in LAND mode diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde new file mode 100644 index 00000000000..8ac512d78d1 --- /dev/null +++ b/ArduCopter/land_detector.pde @@ -0,0 +1,43 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// counter to verify landings +static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed + +// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing) +static bool land_complete_maybe() +{ + return (ap.land_complete || ap.land_complete_maybe); +} + +// update_land_detector - checks if we have landed and updates the ap.land_complete flag +// called at 50hz +static void update_land_detector() +{ + bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX; + bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED; + bool motor_at_lower_limit = motors.limit.throttle_lower; + bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle(); + bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX; + + if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) { + if (!ap.land_complete) { + // increase counter until we hit the trigger then set land complete flag + if( land_detector < LAND_DETECTOR_TRIGGER) { + land_detector++; + }else{ + set_land_complete(true); + land_detector = LAND_DETECTOR_TRIGGER; + } + } + } else { + // we've sensed movement up or down so reset land_detector + land_detector = 0; + // if throttle output is high then clear landing flag + if (motors.get_throttle_out() > get_non_takeoff_throttle()) { + set_land_complete(false); + } + } + + // set land maybe flag + set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER); +} From 3fd38edc4ad594049f69f5a0959fb1479df7ccac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Jan 2015 13:00:57 +0900 Subject: [PATCH 241/254] Copter: restore baro climb rate check to land_detector --- ArduCopter/land_detector.pde | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index 8ac512d78d1..7aa6e4b42a7 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -13,13 +13,13 @@ static bool land_complete_maybe() // called at 50hz static void update_land_detector() { - bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX; - bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED; + bool climb_rate_low = (abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX); + bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z < LAND_SPEED); bool motor_at_lower_limit = motors.limit.throttle_lower; - bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle(); - bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX; + bool throttle_low = (FRAME_CONFIG == HELI_FRAME) || (motors.get_throttle_out() < get_non_takeoff_throttle()); + bool not_rotating_fast = (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX); - if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) { + if (climb_rate_low && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) { if (!ap.land_complete) { // increase counter until we hit the trigger then set land complete flag if( land_detector < LAND_DETECTOR_TRIGGER) { From 0c3f43b2d73d2f8b7593b6f75150e387bdcda82a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Jan 2015 12:41:18 +0900 Subject: [PATCH 242/254] AutoTest: reduce copter throttle when in stabilize Simulated copter is flying too high and causing the land to timeout --- Tools/autotest/arducopter.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 50633f5ee02..aae15ddb3df 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -160,7 +160,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120): save_wp(mavproxy, mav) # switch back to stabilize mode - mavproxy.send('rc 3 1400\n') + mavproxy.send('rc 3 1430\n') mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') @@ -620,7 +620,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120): # switch to stabilize mode mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1400\n') + mavproxy.send('rc 3 1430\n') # fly south 50m print("# Flying south %u meters" % side) @@ -685,7 +685,7 @@ def fly_super_simple(mavproxy, mav, timeout=45): # switch to stabilize mode mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1400\n') + mavproxy.send('rc 3 1430\n') # start copter yawing slowly mavproxy.send('rc 4 1550\n') From b010556f37f5234c3304495a830556296193e187 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 23 Jan 2015 13:13:54 +1100 Subject: [PATCH 243/254] AP_Mission: prevent infinite loop with linked jump commands this prevents a "jump loop" from causing a firmware lockup. Thanks to dellarb for reporting this! --- libraries/AP_Mission/AP_Mission.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 2c6cb1625d1..7a311dbcf2d 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -964,7 +964,8 @@ bool AP_Mission::advance_current_nav_cmd() } // search until we find next nav command or reach end of command list - while (!_flags.nav_cmd_loaded) { + uint8_t max_loops = 64; + while (!_flags.nav_cmd_loaded && --max_loops) { // get next command if (!get_next_cmd(cmd_index, cmd, true)) { return false; @@ -990,6 +991,11 @@ bool AP_Mission::advance_current_nav_cmd() cmd_index = cmd.index+1; } + if (max_loops == 0) { + // the mission is looping + return false; + } + // if we got this far we must have successfully advanced the nav command return true; } @@ -1045,6 +1051,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE; // search until the end of the mission command list + uint8_t max_loops = 64; while(cmd_index < (unsigned)_cmd_total) { // load the next command if (!read_cmd_from_storage(cmd_index, temp_cmd)) { @@ -1055,6 +1062,10 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i // check for do-jump command if (temp_cmd.id == MAV_CMD_DO_JUMP) { + if (max_loops-- == 0) { + return false; + } + // check for invalid target if (temp_cmd.content.jump.target >= (unsigned)_cmd_total) { // To-Do: log an error? From fee41beb59ecf3583a1165aac9bbc21b45903cae Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 30 Jan 2015 16:21:06 +0900 Subject: [PATCH 244/254] Copter: version to AC3.2.1-rc2 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 30499f90be6..820986d2839 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2.1-rc1" +#define THISFIRMWARE "ArduCopter V3.2.1-rc2" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From ef4ca821c54f08d1f4373fbe1a9c6b1210aa400d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 30 Jan 2015 16:21:33 +0900 Subject: [PATCH 245/254] Copter: AC3.2.1-rc2 release notes --- ArduCopter/ReleaseNotes.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 58ce3e91535..05ded2b0c69 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,11 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +ArduCopter 3.2.1-rc2 30-Jan-2015 +Changes from 3.2 +1) Bug Fixes: + a) prevent infinite loop with linked jump commands + b) Pixhawk memory corruption fix when connecting via USB +------------------------------------------------------------------ ArduCopter 3.2.1-rc1 08-Jan-2015 Changes from 3.2 1) Enhancements: From a067a5e623e43ce764b448757a2c6b0a33515317 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 31 Jan 2015 17:23:43 +0900 Subject: [PATCH 246/254] Copter: land detector requires desired climb rate be < -20cm/s --- ArduCopter/config.h | 3 +++ ArduCopter/land_detector.pde | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1f2440de451..2b9be70507a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -503,6 +503,9 @@ #ifndef LAND_DETECTOR_BARO_CLIMBRATE_MAX # define LAND_DETECTOR_BARO_CLIMBRATE_MAX 150 // barometer climb rate must be between -150cm/s ~ +150cm/s #endif +#ifndef LAND_DETECTOR_DESIRED_CLIMBRATE_MAX +# define LAND_DETECTOR_DESIRED_CLIMBRATE_MAX -20 // vehicle desired climb rate must be below -20cm/s +#endif #ifndef LAND_DETECTOR_ROTATION_MAX # define LAND_DETECTOR_ROTATION_MAX 0.50f // vehicle rotation must be below 0.5 rad/sec (=30deg/sec for) vehicle to consider itself landed #endif diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde index 7aa6e4b42a7..b13bc1fff40 100644 --- a/ArduCopter/land_detector.pde +++ b/ArduCopter/land_detector.pde @@ -14,7 +14,7 @@ static bool land_complete_maybe() static void update_land_detector() { bool climb_rate_low = (abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX); - bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z < LAND_SPEED); + bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z <= LAND_DETECTOR_DESIRED_CLIMBRATE_MAX); bool motor_at_lower_limit = motors.limit.throttle_lower; bool throttle_low = (FRAME_CONFIG == HELI_FRAME) || (motors.get_throttle_out() < get_non_takeoff_throttle()); bool not_rotating_fast = (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX); From 90961bb104d37bdf2d82306efe7ffafd20851fab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Feb 2015 22:29:17 +0900 Subject: [PATCH 247/254] Copter: protect against multiple arming messages Protect against the GCS sending multiple arming messages close together which disrupts the gyro calibration --- ArduCopter/motors.pde | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 3f55f25be12..928eb57a725 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -109,6 +109,13 @@ static bool init_arm_motors() // This is used to decide if we should run the ground_start routine // which calibrates the IMU static bool did_ground_start = false; + static bool in_arm_motors = false; + + // exit immediately if already in this function + if (in_arm_motors) { + return false; + } + in_arm_motors = true; // disable cpu failsafe because initialising everything takes a while failsafe_disable(); @@ -155,6 +162,7 @@ static bool init_arm_motors() gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro cal failed")); AP_Notify::flags.armed = false; failsafe_enable(); + in_arm_motors = false; return false; } did_ground_start = true; @@ -182,6 +190,7 @@ static bool init_arm_motors() motors.output_min(); failsafe_enable(); AP_Notify::flags.armed = false; + in_arm_motors = false; return false; } @@ -208,6 +217,9 @@ static bool init_arm_motors() // reenable failsafe failsafe_enable(); + // flag exiting this function + in_arm_motors = false; + // return success return true; } From b32081fae68a25d21047743e56ca488fbef067ce Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Wed, 28 Jan 2015 16:29:18 -0500 Subject: [PATCH 248/254] AC_PosControl: Enable altitude limit checking. --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index e3e3a96fcd0..fad73f1a9b4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -142,8 +142,8 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) { // adjust desired alt if motors have not hit their limits - // To-Do: add check of _limit.pos_up and _limit.pos_down? - if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) { + // To-Do: add check of _limit.pos_down? + if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) { _pos_target.z += climb_rate_cms * dt; } From e28b7c3abe998816206e8bb7b7ef687887f526fd Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Fri, 30 Jan 2015 13:35:23 +0900 Subject: [PATCH 249/254] Copter: update pos_control alt_max from fence at 1hz --- ArduCopter/ArduCopter.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 820986d2839..07926c92d96 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1191,6 +1191,13 @@ static void one_hz_loop() #if AP_TERRAIN_AVAILABLE terrain.update(); #endif + +#if AC_FENCE == ENABLED + // set fence altitude limit in position controller + if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { + pos_control.set_alt_max(fence.get_safe_alt()*100.0f); + } +#endif } // called at 100hz but data from sensor only arrives at 20 Hz From 375b9a5bb192bf43902fbd79e2c2a5c1fe91fbc6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 30 Jan 2015 13:57:37 +0900 Subject: [PATCH 250/254] AC_PosControl: move alt limit to set_alt_target_from_climb_rate The alt limit is instead enforced when the target is set using the set_alt_target_from_climb_rate function Also updated comments --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 14 +++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index fad73f1a9b4..32d1e6d327f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -146,7 +146,13 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) { _pos_target.z += climb_rate_cms * dt; } - + + // do not let target alt get above limit + if (_alt_max > 0 && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + } + _vel_desired.z = climb_rate_cms; } @@ -257,12 +263,6 @@ void AC_PosControl::pos_to_rate_z() _limit.pos_up = false; _limit.pos_down = false; - // do not let target alt get above limit - if (_alt_max > 0 && _pos_target.z > _alt_max) { - _pos_target.z = _alt_max; - _limit.pos_up = true; - } - // calculate altitude error _pos_error.z = _pos_target.z - curr_alt; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 0347c51511e..3896f41f44c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -67,8 +67,8 @@ class AC_PosControl /// /// set_alt_max - sets maximum altitude above home in cm - /// set to zero to disable limit - /// To-Do: update this intermittantly from main code after checking if fence is enabled/disabled + /// only enforced when set_alt_target_from_climb_rate is used + /// set to zero to disable limit void set_alt_max(float alt) { _alt_max = alt; } /// set_speed_z - sets maximum climb and descent rates From 5f118b2936eea4bc4532695ebe4701c75f36c029 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 3 Feb 2015 09:55:44 +0900 Subject: [PATCH 251/254] Copter: update AC3.2.1-rc2 release notes --- ArduCopter/ReleaseNotes.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 05ded2b0c69..3435b2245ab 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,10 +1,12 @@ ArduCopter Release Notes: ------------------------------------------------------------------ ArduCopter 3.2.1-rc2 30-Jan-2015 -Changes from 3.2 +Changes from 3.2.1-rc1 1) Bug Fixes: a) prevent infinite loop with linked jump commands b) Pixhawk memory corruption fix when connecting via USB + c) vehicle stops at fence altitude limit in Loier, AltHold, PosHold + d) protect against multiple arming messages from GCS causing silent gyro calibration failure ------------------------------------------------------------------ ArduCopter 3.2.1-rc1 08-Jan-2015 Changes from 3.2 From dde418477cf45c59a8895316fd57027dc0ed3039 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 11 Feb 2015 12:10:44 +0900 Subject: [PATCH 252/254] Copter: AC3.2.1 release notes No changes from AC3.2.1-rc2 --- ArduCopter/ReleaseNotes.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 3435b2245ab..5c3ba48e398 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,11 +1,11 @@ ArduCopter Release Notes: ------------------------------------------------------------------ -ArduCopter 3.2.1-rc2 30-Jan-2015 +ArduCopter 3.2.1 11-Feb-2015 / 3.2.1-rc2 30-Jan-2015 Changes from 3.2.1-rc1 1) Bug Fixes: a) prevent infinite loop with linked jump commands b) Pixhawk memory corruption fix when connecting via USB - c) vehicle stops at fence altitude limit in Loier, AltHold, PosHold + c) vehicle stops at fence altitude limit in Loiter, AltHold, PosHold d) protect against multiple arming messages from GCS causing silent gyro calibration failure ------------------------------------------------------------------ ArduCopter 3.2.1-rc1 08-Jan-2015 From 36b405fb0b2b3ef77952e8c9f170dbcf1976cb30 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 11 Feb 2015 12:11:01 +0900 Subject: [PATCH 253/254] Copter: version to AC3.2.1 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 07926c92d96..7905870e4f9 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V3.2.1-rc2" +#define THISFIRMWARE "ArduCopter V3.2.1" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by From 03373d35c29d299356a3d80f262f335e8f100814 Mon Sep 17 00:00:00 2001 From: Bill Bonney Date: Fri, 17 Apr 2015 22:15:45 +0000 Subject: [PATCH 254/254] AutoTest: Add ability to run AC3.2.1 in screen --- Tools/autotest/run_in_terminal_window.sh | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/run_in_terminal_window.sh b/Tools/autotest/run_in_terminal_window.sh index 0ff56be3456..bf45464642e 100755 --- a/Tools/autotest/run_in_terminal_window.sh +++ b/Tools/autotest/run_in_terminal_window.sh @@ -13,8 +13,14 @@ elif [ -x /usr/bin/konsole ]; then /usr/bin/konsole --hold -e $* elif [ -x /usr/bin/gnome-terminal ]; then /usr/bin/gnome-terminal -e "$*" +elif [ -n "$STY" ]; then + # We are running inside of screen, try to start it there + /usr/bin/screen -X screen -t $name $* else - echo "ERROR: Please install xterm" - exit 1 + filename="/tmp/$name.log" + echo "Window access not found, logging to $filename" + cmd="$1" + shift + ( run_cmd.sh $cmd $* &>$filename < /dev/null ) & fi exit 0