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 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; } 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/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 298fe7880cf..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-rc5" +#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 @@ -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 //////////////////////////////////////////////////////////////////////////////// @@ -288,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 @@ -388,6 +373,9 @@ 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 + 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; @@ -571,8 +559,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 //////////////////////////////////////////////////////////////////////////////// @@ -795,7 +783,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, 8, 80 }, { update_thr_cruise, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 8, 42 }, @@ -805,7 +793,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 }, @@ -863,7 +851,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 }, @@ -873,7 +861,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 }, @@ -942,7 +930,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"), @@ -957,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 @@ -1089,7 +1074,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(); } } @@ -1102,16 +1087,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)) { + if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { Log_Write_Nav_Tuning(); } } @@ -1126,11 +1111,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 @@ -1160,12 +1145,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(); } @@ -1206,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 @@ -1224,7 +1216,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(); } } @@ -1248,7 +1240,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); } @@ -1334,7 +1326,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); } } @@ -1399,13 +1391,13 @@ 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(); // 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/Attitude.pde b/ArduCopter/Attitude.pde index 4daefa0fa0f..0137a3c6f32 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; @@ -165,13 +165,16 @@ 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 - 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/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 798f8ef1489..e6e611824af 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -199,11 +199,14 @@ 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() || !ins.gyro_calibrated_ok_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()) { + if (ahrs.initialised() && !ahrs.healthy()) { // AHRS subsystem is unhealthy control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } @@ -221,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; @@ -276,8 +280,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, @@ -479,10 +482,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: @@ -877,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)) { @@ -897,6 +909,13 @@ 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 + send_text_P(SEVERITY_LOW, PSTR("Frame: " FRAME_CONFIG_STRING)); handle_param_request_list(msg); break; } @@ -1044,12 +1063,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; @@ -1091,10 +1110,11 @@ 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(); + // reset ahrs gyro bias + ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } if (packet.param3 == 1) { @@ -1138,14 +1158,18 @@ 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)) { - init_arm_motors(); + if(ap.pre_arm_check && arm_checks(true, true)) { + 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; } - } 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 { @@ -1279,11 +1303,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 b59e5ada488..3142665f042 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 = { @@ -464,13 +464,14 @@ 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 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(), @@ -479,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)); @@ -530,7 +533,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 @@ -548,7 +551,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, @@ -567,7 +570,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, @@ -586,7 +589,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, @@ -605,7 +608,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, @@ -624,7 +627,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, @@ -685,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), @@ -715,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)); @@ -746,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/Parameters.h b/ArduCopter/Parameters.h index 72c68991428..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 @@ -119,7 +119,11 @@ 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, + k_param_optflow, + k_param_dcmcheck_thresh, // 59 + k_param_log_bitmask, // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -365,6 +369,7 @@ class Parameters { AP_Int16 failsafe_throttle_value; AP_Int16 throttle_cruise; AP_Int16 throttle_mid; + AP_Int16 throttle_deadzone; // Flight modes // @@ -378,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; @@ -390,6 +395,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 136c4cc1961..51c957cbaa5 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 @@ -345,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,-6146:NearlyAll-AC315,43006:NearlyAll,131070:All+DisarmedLogging,0:Disabled // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -381,7 +390,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), @@ -447,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 @@ -589,7 +605,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), @@ -799,7 +815,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 @@ -993,7 +1009,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_ @@ -1126,12 +1142,13 @@ 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) { 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")); } diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 879b888bd6d..5c3ba48e398 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,125 @@ ArduCopter Release Notes: ------------------------------------------------------------------ +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 Loiter, 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 +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 + 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: + 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 +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 +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 +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 +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) +------------------------------------------------------------------ +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: + 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 +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) 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 + 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 1) Pixhawk's max num waypoints increased to 718 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 d2410637f7b..6418b8f467e 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); } @@ -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); @@ -275,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 @@ -782,6 +772,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); } @@ -913,7 +904,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/config.h b/ArduCopter/config.h index eae4043fc84..2b9be70507a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -84,6 +84,13 @@ #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 + +// 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 @@ -110,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 @@ -273,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 @@ -299,15 +328,33 @@ #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_CM + # define PREARM_MAX_ALT_DISPARITY_CM 200 // 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 #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 + +// 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 +// EKF & DCM 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 +#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 ////////////////////////////////////////////////////////////////////////////// @@ -338,6 +385,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) @@ -442,11 +494,26 @@ #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 +#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 #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 #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 @@ -584,7 +651,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 @@ -654,8 +721,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 @@ -664,7 +731,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 800 #endif // default maximum vertical velocity and acceleration the pilot may request @@ -684,20 +765,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 // 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; diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index e4dddc22ead..6f28aa72298 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; } @@ -287,6 +288,11 @@ static void auto_land_run() return; } + // relax loiter targets if we might be landed + if (land_complete_maybe()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { @@ -309,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 @@ -470,7 +476,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; @@ -481,7 +487,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 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 diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 30f5fd9f176..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 } } @@ -140,13 +147,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; } diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index b85e44c84bf..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; static bool land_with_gps; static uint32_t land_start_time; @@ -61,7 +59,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 @@ -73,6 +71,11 @@ static void land_gps_run() return; } + // relax loiter target if we might be landed + if (land_complete_maybe()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { @@ -107,7 +110,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(); } @@ -126,7 +129,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 @@ -165,7 +168,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(); } @@ -187,33 +190,6 @@ 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() -{ - // 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) - if( land_detector < LAND_DETECTOR_TRIGGER) { - land_detector++; - }else{ - set_land_complete(true); - land_detector = 0; - } - } - } else if ((motors.get_throttle_out() > get_non_takeoff_throttle()) || failsafe.radio) { - // we've sensed movement up or down so reset land_detector - land_detector = 0; - if(ap.land_complete) { - 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 // 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 @@ -228,3 +204,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); +} diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 3883342e6f6..f606636adc9 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_complete_maybe()) { + 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..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 @@ -183,6 +183,11 @@ static void poshold_run() } } + // relax loiter target if we might be landed + if (land_complete_maybe()) { + 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 88689b85b5e..7fc52232328 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -325,15 +325,35 @@ 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 && (ap.throttle_zero || 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; } + // relax loiter target if we might be landed + if (land_complete_maybe()) { + wp_nav.loiter_soften_for_landing(); + } + // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { @@ -357,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 @@ -365,18 +385,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 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/defines.h b/ArduCopter/defines.h index 8a19fa343b0..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 @@ -263,6 +265,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 @@ -275,6 +279,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 @@ -329,8 +334,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 +361,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..da73ac74ea7 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -11,61 +11,52 @@ # 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 +// 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() { - // 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) { + 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() || 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; } - // 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 - // 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 @@ -76,25 +67,29 @@ 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 (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(); } } } 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_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(); } @@ -104,18 +99,47 @@ 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 } +// 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 + return false; +#endif +} + + // 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; } @@ -126,7 +150,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 +173,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); } 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..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) && g.rc_3.control_in == 0 && !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 diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 35af7ca57a2..a87d1b714d1 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 @@ -252,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; @@ -262,6 +267,15 @@ static bool manual_flight_mode(uint8_t mode) { return false; } +// 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; +} + // // print_flight_mode - prints flight mode to serial port. // @@ -320,8 +334,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(); -} diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index 374cdfcab0d..4f3feb8d1b6 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; } @@ -31,19 +34,33 @@ 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); - - // run attitude controller - attitude_control.rate_bf_roll_pitch_yaw(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); + // 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); + } // output pilot's throttle without angle boost 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 diff --git a/ArduCopter/land_detector.pde b/ArduCopter/land_detector.pde new file mode 100644 index 00000000000..b13bc1fff40 --- /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_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_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); + + 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) { + 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); +} 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 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index fbdbbf3cfd2..928eb57a725 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -5,12 +5,13 @@ #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() { static int16_t arming_counter; - bool allow_arming = false; // ensure throttle is down if (g.rc_3.control_in > 0) { @@ -18,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 @@ -56,8 +33,12 @@ 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)) { - init_arm_motors(); + if(ap.pre_arm_check && arm_checks(true,false)) { + 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; @@ -68,10 +49,16 @@ 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 }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 ) { @@ -94,18 +81,14 @@ 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) { + if (!motors.armed() || !ap.throttle_zero) { auto_disarming_counter = 0; return; } // 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) { @@ -118,13 +101,21 @@ 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. // 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(); @@ -132,6 +123,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 @@ -162,8 +156,16 @@ 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; + failsafe_enable(); + in_arm_motors = false; + return false; + } + did_ground_start = true; } // fast baro calibration to reset ground pressure @@ -188,8 +190,8 @@ static void init_arm_motors() motors.output_min(); failsafe_enable(); AP_Notify::flags.armed = false; - AP_Notify::flags.arming_failed = false; - return; + in_arm_motors = false; + return false; } #if SPRAYER == ENABLED @@ -197,6 +199,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(); @@ -211,13 +216,26 @@ static void init_arm_motors() // reenable failsafe failsafe_enable(); + + // flag exiting this function + in_arm_motors = false; + + // return success + return true; } // 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; } @@ -247,7 +265,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")); } @@ -266,7 +284,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")); } @@ -290,21 +308,40 @@ 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 - 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 @@ -317,13 +354,63 @@ 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: Accels not healthy")); + } + 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) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros not healthy")); } 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) { + 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 @@ -418,13 +505,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; } @@ -433,14 +546,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; } @@ -449,17 +565,38 @@ 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; } // 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 arming_from_gcs) { + // always check if the current mode allows arming + if (!mode_allows_arming(control_mode, arming_from_gcs)) { + 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; @@ -467,7 +604,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_CM) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity")); } @@ -475,11 +612,9 @@ static bool arm_checks(bool display_failure) } } - // 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 @@ -546,6 +681,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(); @@ -557,7 +693,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); 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; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index d3914d34ffb..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 @@ -62,13 +65,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 @@ -97,9 +100,11 @@ void output_min() static void read_radio() { - static uint32_t last_update = 0; + static uint32_t last_update_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); @@ -107,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]); @@ -114,6 +120,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; @@ -122,7 +135,7 @@ static void read_radio() // update output on any aux channels, for manual passthru RC_Channel_aux::output_ch_all(); }else{ - uint32_t elapsed = millis() - last_update; + uint32_t elapsed = tnow_ms - last_update_ms; // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) && (g.failsafe_throttle && motors.armed() && !failsafe.radio)) { @@ -174,6 +187,22 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm) } } +#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 +// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control_in +static void set_throttle_zero_flag(int16_t throttle_control) +{ + static uint32_t last_nonzero_throttle_ms = 0; + uint32_t tnow_ms = millis(); + + // if non-zero throttle immediately set as non-zero + if (throttle_control > 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++) { diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 03aea807820..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 (g.log_bitmask & MASK_LOG_IMU) { + 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/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 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index ee00e4a0d36..5aa94634943 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -288,12 +288,16 @@ 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); } cliSerial->print_P(PSTR("\nReady to FLY ")); + + // flag that initialisation has completed + ap.initialised = true; } @@ -316,11 +320,17 @@ 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); // set landed flag set_land_complete(true); + set_land_complete_maybe(true); } // returns true if the GPS is ok and home position is set @@ -346,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{ @@ -354,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 @@ -399,3 +409,24 @@ 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 LOGGING_ENABLED == ENABLED + 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 + start_logging(); + } + return ret; +#else + return false; +#endif +} 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()); } 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), 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/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') 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 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 diff --git a/Tools/scripts/build_all_travis.sh b/Tools/scripts/build_all_travis.sh new file mode 100755 index 00000000000..64ac54a3d11 --- /dev/null +++ b/Tools/scripts/build_all_travis.sh @@ -0,0 +1,31 @@ +#!/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 ArduCopter +make configure +for b in all apm2 sitl; do + pwd + make clean + make $b -j4 +done +popd + +for d in ArduCopter; do + pushd $d + make clean + make sitl -j4 + make clean + make px4-cleandep + make px4-v2 + popd +done + +exit 0 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 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index d4c795f8098..4a2aae82ca9 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 = { @@ -382,18 +380,19 @@ 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); + _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); } } @@ -432,12 +431,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; } // @@ -668,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); } // 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index f9ab49a534b..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), @@ -61,6 +63,65 @@ 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; + 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) { + _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,8 +131,13 @@ 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? - rate_bf_to_motor_roll_pitch(_rate_bf_target.x, _rate_bf_target.y); + // 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)); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 4fe6aa4a28f..d9c4db6c851 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -26,11 +26,15 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { ) : 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), + _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(); @@ -38,6 +42,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 @@ -47,10 +54,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 roll & pitch input directly to swash-plate } _flags_heli; // @@ -68,6 +76,7 @@ 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); + // 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. @@ -75,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 diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 34b066cb7e6..32d1e6d327f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -84,6 +84,10 @@ 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 + _vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ); + _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ); } /// set_speed_z - sets maximum climb and descent rates @@ -118,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)) { @@ -133,13 +139,21 @@ 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)) { + // 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; } + + // 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; } // get_alt_error - returns altitude error in cm @@ -249,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; @@ -334,10 +342,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 @@ -365,11 +375,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 @@ -469,7 +479,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; @@ -545,6 +555,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 diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 14ce41faf1a..3896f41f44c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -40,6 +40,9 @@ #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 { public: @@ -64,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 @@ -106,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); + /// 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(); } @@ -367,6 +371,8 @@ 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 uint8_t _vel_xyz_step; // used to decide which portion of velocity controller to run during this iteration diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b7a7b2cfbc5..dee9690c66b 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) { @@ -331,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 @@ -404,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 { @@ -695,7 +732,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 +744,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 } } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 0342889f420..60bcd095af3 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); @@ -132,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; 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 a1f96a482b8..5e30e51eee9 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, @@ -179,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; @@ -338,6 +344,9 @@ class AP_AHRS // is the AHRS subsystem healthy? virtual bool healthy(void) = 0; + // true if the AHRS has completed initialisation + virtual bool initialised(void) const { return true; }; + protected: AHRS_VehicleClass _vehicle_class; @@ -364,6 +373,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 9f79bcd45c6..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 @@ -143,7 +152,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 +170,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 +251,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 +371,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 */ @@ -442,6 +451,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 @@ -684,7 +698,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 { @@ -715,6 +729,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. @@ -837,12 +856,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_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 2098f7b04e5..60f384e8823 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -50,16 +50,35 @@ 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(); + + // reset the EKF gyro bias states + EKF.resetGyroBias(); +} + 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; + update_cd_values(); + AP_AHRS_DCM::update(); // keep DCM attitude available for get_secondary_attitude() _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(); } @@ -78,11 +97,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() @@ -166,6 +182,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(); } @@ -263,5 +282,12 @@ bool AP_AHRS_NavEKF::healthy(void) return AP_AHRS_DCM::healthy(); } +// true if the AHRS has completed initialisation +bool AP_AHRS_NavEKF::initialised(void) const +{ + // 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 563535bcae5..bc065866fa4 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 { @@ -48,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); @@ -95,6 +100,9 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM // is the AHRS subsystem healthy? bool healthy(void); + // true if the AHRS has completed initialisation + bool initialised(void) const; + private: bool using_EKF(void) const; 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_Compass/AP_Compass_VRBRAIN.cpp b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp index fe42a639294..1abe787a141 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); } @@ -108,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]); - +#endif if (_is_external[i]) { // add user selectable orientation _sum[i].rotate((enum Rotation)_orientation.get()); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 02920acc1a5..33b9ac304c8 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 @@ -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 ae115dcaa98..1a0af229278 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: @@ -106,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 diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index adbcfcad66b..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); } @@ -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 #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: 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 diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 401b2b0a9da..cea2d197ced 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -215,13 +215,15 @@ #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 #define HAL_SERIAL0_BAUD_DEFAULT 115200 +#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_NONE #else #error "Unknown CONFIG_HAL_BOARD type" 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/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; 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); diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp index 1fc54b591a2..7ac62108e67 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) @@ -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); } 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); } 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 }, 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 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" 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) 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); } diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index d793cf95cb1..585c82f873f 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -25,12 +25,12 @@ 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); - _ahrs.get_velocity_NED(_velocity_cm); + _ahrs_ekf.get_NavEKF().getVelNED(_velocity_cm); _velocity_cm *= 100; // convert to cm/s // InertialNav is NEU 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__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index d1bbac8333c..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,161 +217,374 @@ 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; + + if (_gyro_count == 0 && _accel_count == 0) { + // detect available backends. Only called once + _detect_backends(); + } + + _product_id = 0; // FIX - // check scaling + // 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++; + } } -// save parameters to eeprom -void AP_InertialSensor::_save_parameters() + +/* + detect available backends for this board + */ +void +AP_InertialSensor::_detect_backends(void) { - _product_id.save(); - for (uint8_t i=0; ipanic(PSTR("No INS backends available")); } + + // set the product ID to the ID of the first backend + _product_id.set(_backends[0]->product_id()); } void -AP_InertialSensor::init_gyro() +AP_InertialSensor::init_accel() { - _init_gyro(); + _init_accel(); // save calibration _save_parameters(); + + check_3D_calibration(); } -void -AP_InertialSensor::_init_gyro() +#if !defined( __AVR_ATmega1280__ ) +// calibrate_accel - perform accelerometer calibration including providing user +// instructions and feedback Gauss-Newton accel calibration routines borrowed +// from Rolfe Schmidt blog post describing the method: +// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/ +// original sketch available at +// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde +bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact, + float &trim_roll, + float &trim_pitch) { - uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES); - Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES]; - float best_diff[INS_MAX_INSTANCES]; - bool converged[INS_MAX_INSTANCES]; - - // cold start - hal.console->print_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; kdelay(5); + hal.scheduler->delay(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++; } - // stop flashing leds - AP_Notify::flags.initialising = false; + if (num_ok == num_accels) { + interact->printf_P(PSTR("Calibration successful\n")); - if (num_converged == num_gyros) { - // all OK - return; + 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]; +failed: + interact->printf_P(PSTR("Calibration FAILED\n")); + // restore original scaling and offsets + for (uint8_t k=0; k 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 +{ + for (uint8_t i=0; i 0); +} + void AP_InertialSensor::_init_accel() { @@ -360,150 +685,109 @@ AP_InertialSensor::_init_accel() } -#if !defined( __AVR_ATmega1280__ ) -// calibrate_accel - perform accelerometer calibration including providing user -// instructions and feedback Gauss-Newton accel calibration routines borrowed -// from Rolfe Schmidt blog post describing the method: -// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/ -// original sketch available at -// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde -bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact, - float &trim_roll, - float &trim_pitch) +void +AP_InertialSensor::_init_gyro() { - 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; + uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES); + Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES]; + float best_diff[INS_MAX_INSTANCES]; + bool converged[INS_MAX_INSTANCES]; - 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 30 seconds + // if the gyros are stable, we should get it in 1 second + for (int16_t j = 0; j <= 30*4 && 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]; + // flag calibration as failed for this gyro + _gyro_cal_ok[k] = false; } } - // 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 +963,187 @@ 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; 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 78f13baafce..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,61 +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); - // 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()); } - virtual uint8_t get_gyro_count(void) const { return 1; }; + 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]; } - virtual bool get_accel_health(uint8_t instance) const { return true; } - bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } - virtual uint8_t get_accel_count(void) const { return 1; }; + // multi-device interface + 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; + 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; + + 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; + 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[]; @@ -165,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 @@ -190,50 +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]; + + // 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_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_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 d50691c4023..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->micros(); - 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_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__ 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(); diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 017e0ea6c0f..9473c815bd9 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_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 152e8e68c29..404f31dc5c6 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_90 = 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..ae5399df326 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_90: { + float tmpx = x; + float tmpy = y; + float tmpz = z; + 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; + } } } diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 69381352649..7a311dbcf2d 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; + } } } @@ -958,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; @@ -984,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; } @@ -1039,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)) { @@ -1049,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? diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index ef3c0ab8221..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; } @@ -385,17 +384,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_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 diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 0dbc57e2b90..315925ac1c3 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 @@ -60,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: @@ -170,7 +177,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 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(); diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 931d20a17c9..017789f5c59 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 @@ -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 = { @@ -346,6 +349,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 +357,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)); } @@ -369,7 +372,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 @@ -401,15 +404,20 @@ 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 - states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x; - states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y; + // write to state vector and compensate for GPS latency + 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); + } + // 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]; } } @@ -423,20 +431,17 @@ 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 + 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 = velNED; } } } @@ -447,9 +452,13 @@ 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.z = -hgtMea; + } } // this function is used to initialise the filter whilst moving, using the AHRS DCM solution @@ -464,14 +473,20 @@ 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; - 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; @@ -501,6 +516,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 +674,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,39 +684,24 @@ 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 (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)) { - // 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; } - // command fusion of height data - if (newDataHgt) - { - // reset data arrived flag - newDataHgt = false; - // enable fusion - fuseHgtData = true; - } else if (hal.scheduler->millis() - 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; - } - } 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 @@ -712,25 +712,43 @@ void NavEKF::SelectVelPosFusion() fusePosData = false; } fuseVelData = false; - fuseHgtData = true; } // check for and read new height data readHgtData(); - // perform fusion as commanded, and in accordance with specified time intervals + // 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 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 @@ -743,9 +761,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,8 +777,10 @@ void NavEKF::SelectMagFusion() bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) { - MAGmsecPrev = IMUmsec; 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 +789,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]; + } + } + } } @@ -782,7 +811,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 +819,7 @@ void NavEKF::SelectTasFusion() if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow)) { FuseAirspeed(); - TASmsecPrev = IMUmsec; + TASmsecPrev = imuSampleTime_ms; tasDataWaiting = false; } } @@ -805,9 +834,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; } } @@ -870,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 @@ -948,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. @@ -988,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); @@ -1641,20 +1669,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]))) { @@ -1675,16 +1703,16 @@ 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 maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime))); + 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); // 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]; @@ -1721,7 +1749,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) { @@ -1745,11 +1773,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 +1802,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(); @@ -1814,12 +1842,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]; } @@ -1872,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 } @@ -1890,9 +1921,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 +1956,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 +2279,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(); @@ -2407,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++) @@ -2485,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; @@ -2601,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 @@ -2678,8 +2726,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 +2746,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; } @@ -2737,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; } @@ -2767,9 +2810,19 @@ 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; +} + +// 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(INIT_GYRO_BIAS_UNCERTAINTY * 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 @@ -2780,33 +2833,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 @@ -2814,10 +2863,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; } @@ -2892,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 @@ -2922,7 +2971,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 +3042,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,12 +3097,21 @@ 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(); + // 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 { // <= 4 satellites + 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 @@ -3079,14 +3137,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 +3164,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 +3186,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; } @@ -3204,24 +3262,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 @@ -3252,12 +3306,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); @@ -3270,8 +3324,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); } @@ -3308,29 +3361,31 @@ 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; + + gpsNoiseScaler = 1.0f; 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; @@ -3352,6 +3407,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 @@ -3382,8 +3441,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)) { @@ -3415,15 +3474,15 @@ 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; - 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 d2b6b5dde70..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; @@ -135,6 +138,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 @@ -343,11 +350,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 @@ -360,6 +367,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 @@ -418,8 +426,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 @@ -427,18 +433,16 @@ 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 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) @@ -467,6 +471,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; @@ -510,9 +529,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; }; 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; 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); 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) 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; 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 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; diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index dcbd6ec9b98..7fb362a4d35 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 @@ -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 diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index a77c8866c0f..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 { @@ -392,6 +393,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), \ @@ -409,7 +432,7 @@ struct PACKED log_TERRAIN { { 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), \ @@ -440,7 +463,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 +501,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 diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 13a63ca3325..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), @@ -105,9 +106,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; @@ -117,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; } @@ -184,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; @@ -221,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; } @@ -300,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) { @@ -314,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; } @@ -397,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; @@ -412,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; @@ -441,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) { @@ -573,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 0eda11f5988..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; /* @@ -67,7 +68,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; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 96500451fd1..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]); @@ -763,6 +766,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)); } 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); } 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 ) 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 ) 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 diff --git a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM index a3d7f5cfe67..b3e853914ec 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APM @@ -109,6 +109,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 HMC5983 External EXP" if hmc5983 -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..25fc31fd29d 100644 --- a/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS +++ b/mk/VRBRAIN/ROMFS_VRBRAIN51_APM/init.d/rc.APMNS @@ -82,6 +82,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 HMC5983 External EXP" if hmc5983 -X start then 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/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 diff --git a/mk/vrbrain_targets.mk b/mk/vrbrain_targets.mk index 956eaf64417..ca789a8083f 100644 --- a/mk/vrbrain_targets.mk +++ b/mk/vrbrain_targets.mk @@ -30,17 +30,28 @@ 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 + 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)),) +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 @@ -100,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) @@ -114,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) @@ -128,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) @@ -142,10 +223,17 @@ 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-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 vrbrain-cleandep: clean @@ -159,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 @@ -167,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 @@ -186,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)