From 7689315ba2a42449207ca68c1063cb7fea95d936 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Apr 2016 20:37:04 +0900 Subject: [PATCH] Copter: rename sonar to rangefinder --- ArduCopter/ArduCopter.cpp | 6 +++--- ArduCopter/Attitude.cpp | 18 ++++++++-------- ArduCopter/Copter.cpp | 8 +++---- ArduCopter/Copter.h | 23 ++++++++++---------- ArduCopter/GCS_Mavlink.cpp | 14 ++++++------- ArduCopter/Log.cpp | 8 +++---- ArduCopter/Parameters.cpp | 8 +++---- ArduCopter/Parameters.h | 10 ++++----- ArduCopter/arming_checks.cpp | 2 +- ArduCopter/config.h | 24 ++++++++++----------- ArduCopter/control_althold.cpp | 4 ++-- ArduCopter/control_circle.cpp | 4 ++-- ArduCopter/control_land.cpp | 8 +++---- ArduCopter/control_loiter.cpp | 4 ++-- ArduCopter/control_poshold.cpp | 4 ++-- ArduCopter/control_sport.cpp | 4 ++-- ArduCopter/defines.h | 4 ++-- ArduCopter/heli.cpp | 4 ++-- ArduCopter/precision_landing.cpp | 4 ++-- ArduCopter/sensors.cpp | 36 ++++++++++++++++---------------- ArduCopter/switches.cpp | 10 ++++----- ArduCopter/system.cpp | 6 ++---- ArduCopter/terrain.cpp | 2 +- ArduCopter/test.cpp | 14 ++++++------- ArduCopter/tuning.cpp | 6 +++--- 25 files changed, 116 insertions(+), 119 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 4f2de95d6f..18bbc4da0d 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -607,14 +607,14 @@ void Copter::read_AHRS(void) ahrs.update(); } -// read baro and sonar altitude at 10hz +// read baro and rangefinder altitude at 10hz void Copter::update_altitude() { // read in baro altitude read_barometer(); - // read in sonar altitude - sonar_alt = read_sonar(); + // read in rangefinder altitude + rangefinder_alt = read_rangefinder(); // write altitude info to dataflash logs if (should_log(MASK_LOG_CTUN)) { diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index bca814cc9e..70f21361a3 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -254,26 +254,26 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current uint32_t now = millis(); // reset target altitude if this controller has just been engaged - if (now - last_call_ms > SONAR_TIMEOUT_MS) { - target_sonar_alt = sonar_alt + current_alt_target - current_alt; + if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) { + target_rangefinder_alt = rangefinder_alt + current_alt_target - current_alt; } last_call_ms = now; - // adjust sonar target alt if motors have not hit their limits + // adjust rangefinder target alt if motors have not hit their limits if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) { - target_sonar_alt += target_rate * dt; + target_rangefinder_alt += target_rate * dt; } // do not let target altitude get too far from current altitude above ground // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition - target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z()); + target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_alt-pos_control.get_leash_down_z(),rangefinder_alt+pos_control.get_leash_up_z()); - // calc desired velocity correction from target sonar alt vs actual sonar alt (remove the error already passed to Altitude controller to avoid oscillations) - distance_error = (target_sonar_alt - sonar_alt) - (current_alt_target - current_alt); - velocity_correction = distance_error * g.sonar_gain; + // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) + distance_error = (target_rangefinder_alt - rangefinder_alt) - (current_alt_target - current_alt); + velocity_correction = distance_error * g.rangefinder_gain; velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX); - // return combined pilot climb rate + rate to correct sonar alt error + // return combined pilot climb rate + rate to correct rangefinder alt error return (target_rate + velocity_correction); } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 133c215831..fa0435e3e2 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Copter::Copter(void) : DataFlash{FIRMWARE_STRING}, flight_modes(&g.flight_mode1), - sonar_enabled(true), + rangefinder_enabled(true), mission(ahrs, FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &), @@ -60,9 +60,9 @@ Copter::Copter(void) : frsky_telemetry(ahrs, battery), #endif climb_rate(0), - sonar_alt(0), - sonar_alt_health(0), - target_sonar_alt(0.0f), + rangefinder_alt(0), + rangefinder_alt_health(0), + target_rangefinder_alt(0.0f), baro_alt(0), baro_climbrate(0.0f), land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 92b9e2623f..59c55686ee 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -170,16 +170,16 @@ private: AP_InertialSensor ins; #if RANGEFINDER_ENABLED == ENABLED - RangeFinder sonar {serial_manager}; - bool sonar_enabled; // enable user switch for sonar + RangeFinder rangefinder {serial_manager}; + bool rangefinder_enabled; // enable user switch for rangefinder #endif AP_RPM rpm_sensor; // Inertial Navigation EKF - NavEKF EKF{&ahrs, barometer, sonar}; - NavEKF2 EKF2{&ahrs, barometer, sonar}; - AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; + NavEKF EKF{&ahrs, barometer, rangefinder}; + NavEKF2 EKF2{&ahrs, barometer, rangefinder}; + AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; @@ -403,10 +403,9 @@ private: // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; - // The altitude as reported by Sonar in cm - Values are 20 to 700 generally. - int16_t sonar_alt; - uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar - float target_sonar_alt; // desired altitude in cm above the ground + int16_t rangefinder_alt; // altitude as reported by the rangefinder in cm + uint8_t rangefinder_alt_health; // true if we can trust the altitude from the rangefinder + float target_rangefinder_alt; // desired altitude in cm above the ground int32_t baro_alt; // barometer altitude in cm above home float baro_climbrate; // barometer climbrate in cm/s LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests @@ -946,8 +945,8 @@ private: void radio_passthrough_to_motors(); void init_barometer(bool full_calibration); void read_barometer(void); - void init_sonar(void); - int16_t read_sonar(void); + void init_rangefinder(void); + int16_t read_rangefinder(void); void init_compass(); void init_optflow(); void update_optical_flow(void); @@ -1076,7 +1075,7 @@ public: int8_t test_optflow(uint8_t argc, const Menu::arg *argv); int8_t test_relay(uint8_t argc, const Menu::arg *argv); int8_t test_shell(uint8_t argc, const Menu::arg *argv); - int8_t test_sonar(uint8_t argc, const Menu::arg *argv); + int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv); int8_t reboot_board(uint8_t argc, const Menu::arg *argv); }; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d175af2cc5..d9f97d9f78 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -250,10 +250,10 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) #endif #if RANGEFINDER_ENABLED == ENABLED - if (sonar.num_sensors() > 0) { + if (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (sonar.has_data()) { + if (rangefinder.has_data()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } @@ -412,14 +412,14 @@ void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan) #if RANGEFINDER_ENABLED == ENABLED void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan) { - // exit immediately if sonar is disabled - if (!sonar.has_data()) { + // exit immediately if rangefinder is disabled + if (!rangefinder.has_data()) { return; } mavlink_msg_rangefinder_send( chan, - sonar.distance_cm() * 0.01f, - sonar.voltage_mv() * 0.001f); + rangefinder.distance_cm() * 0.01f, + rangefinder.voltage_mv() * 0.001f); } #endif @@ -1847,7 +1847,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_DISTANCE_SENSOR: { result = MAV_RESULT_ACCEPTED; - copter.sonar.handle_msg(msg); + copter.rangefinder.handle_msg(msg); break; } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 5790586f47..448ddfab98 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -305,8 +305,8 @@ struct PACKED log_Control_Tuning { float desired_alt; float inav_alt; int32_t baro_alt; - int16_t desired_sonar_alt; - int16_t sonar_alt; + int16_t desired_rangefinder_alt; + int16_t rangefinder_alt; float terr_alt; int16_t desired_climb_rate; int16_t climb_rate; @@ -330,8 +330,8 @@ void Copter::Log_Write_Control_Tuning() desired_alt : pos_control.get_alt_target() / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : baro_alt, - desired_sonar_alt : (int16_t)target_sonar_alt, - sonar_alt : sonar_alt, + desired_rangefinder_alt : (int16_t)target_rangefinder_alt, + rangefinder_alt : rangefinder_alt, terr_alt : terr_alt, desired_climb_rate : (int16_t)pos_control.get_vel_target_z(), climb_rate : climb_rate diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 0785673f16..a972fce510 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -155,7 +155,7 @@ const AP_Param::Info Copter::var_info[] = { // @Range: 0.01 2.0 // @Increment: 0.01 // @User: Standard - GSCALAR(sonar_gain, "RNGFND_GAIN", SONAR_GAIN_DEFAULT), + GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT), // @Param: FS_BATT_ENABLE // @DisplayName: Battery Failsafe Enable @@ -457,8 +457,8 @@ const AP_Param::Info Copter::var_info[] = { // @Param: ARMING_CHECK // @DisplayName: Arming check // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS - // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Sonar, -65:Skip RC, 127:Skip Voltage - // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Sonar,6:RC,7:Voltage + // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Params/Rangefinder, -65:Skip RC, 127:Skip Voltage + // @Bitmask: 0:All,1:Baro,2:Compass,3:GPS,4:INS,5:Parameters+Rangefinder,6:RC,7:Voltage // @User: Standard GSCALAR(arming_check, "ARMING_CHECK", ARMING_CHECK_ALL), @@ -898,7 +898,7 @@ const AP_Param::Info Copter::var_info[] = { #if RANGEFINDER_ENABLED == ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp - GOBJECT(sonar, "RNGFND", RangeFinder), + GOBJECT(rangefinder, "RNGFND", RangeFinder), #endif #if AP_TERRAIN_AVAILABLE && AC_TERRAIN diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9364973977..46371aee01 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -98,7 +98,7 @@ public: k_param_acro_trainer, k_param_pilot_velocity_z_max, k_param_circle_rate, // deprecated - remove - k_param_sonar_gain, + k_param_rangefinder_gain, k_param_ch8_option, k_param_arming_check, k_param_sprayer, @@ -121,7 +121,7 @@ public: k_param_serial1_baud, // deprecated - remove k_param_serial2_baud, // deprecated - remove k_param_land_repositioning, - k_param_sonar, // sonar object + k_param_rangefinder, // rangefinder object k_param_fs_ekf_thresh, k_param_terrain, k_param_acro_expo, @@ -231,13 +231,13 @@ public: k_param_pack_capacity, // deprecated - can be deleted k_param_compass_enabled, k_param_compass, - k_param_sonar_enabled_old, // deprecated + k_param_rangefinder_enabled_old, // deprecated k_param_frame_orientation, k_param_optflow_enabled, // deprecated k_param_fs_batt_voltage, k_param_ch7_option, k_param_auto_slew_rate, // deprecated - can be deleted - k_param_sonar_type_old, // deprecated + k_param_rangefinder_type_old, // deprecated k_param_super_simple = 155, k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change k_param_copter_leds_mode, // deprecated - remove with next eeprom number change @@ -383,7 +383,7 @@ public: AP_Int16 rtl_altitude; AP_Int16 rtl_speed_cms; AP_Float rtl_cone_slope; - AP_Float sonar_gain; + AP_Float rangefinder_gain; AP_Int8 failsafe_battery_enabled; // battery failsafe enabled AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 0a7775865d..a612f8eba9 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -319,7 +319,7 @@ bool Copter::pre_arm_checks(bool display_failure) #if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED // check range finder if optflow enabled - if (optflow.enabled() && !sonar.pre_arm_check()) { + if (optflow.enabled() && !rangefinder.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 94075e60cd..6ae6c3912e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -122,35 +122,35 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Sonar +// Rangefinder // #ifndef RANGEFINDER_ENABLED # define RANGEFINDER_ENABLED ENABLED #endif -#ifndef SONAR_ALT_HEALTH_MAX - # define SONAR_ALT_HEALTH_MAX 3 // number of good reads that indicates a healthy sonar +#ifndef RANGEFINDER_HEALTH_MAX + # define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder #endif -#ifndef SONAR_RELIABLE_DISTANCE_PCT - # define SONAR_RELIABLE_DISTANCE_PCT 0.60f // we trust the sonar out to 60% of it's maximum range +#ifndef RANGEFINDER_RELIABLE_DISTANCE_PCT + # define RANGEFINDER_RELIABLE_DISTANCE_PCT 0.60f // we trust the rangefinder out to 60% of it's maximum range #endif -#ifndef SONAR_GAIN_DEFAULT - # define SONAR_GAIN_DEFAULT 0.8f // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction) +#ifndef RANGEFINDER_GAIN_DEFAULT + # define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) #endif #ifndef THR_SURFACE_TRACKING_VELZ_MAX - # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with sonar + # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder #endif -#ifndef SONAR_TIMEOUT_MS - # define SONAR_TIMEOUT_MS 1000 // desired sonar alt will reset to current sonar alt after this many milliseconds without a good sonar alt +#ifndef RANGEFINDER_TIMEOUT_MS + # define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt #endif -#ifndef SONAR_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF - # define SONAR_TILT_CORRECTION DISABLED +#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF + # define RANGEFINDER_TILT_CORRECTION DISABLED #endif diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 5a5fc36d10..a9da150e95 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -160,8 +160,8 @@ void Copter::althold_run() attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // call throttle controller - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - // if sonar is ok, use surface tracking + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 145dc7845d..e1a927ef5f 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -91,8 +91,8 @@ void Copter::circle_run() } // run altitude controller - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - // if sonar is ok, use surface tracking + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index e4bb636e1b..75f556c884 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -234,9 +234,9 @@ void Copter::land_nogps_run() float Copter::get_land_descent_speed() { #if RANGEFINDER_ENABLED == ENABLED - bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good); + bool rangefinder_ok = rangefinder_enabled && (rangefinder.status() == RangeFinder::RangeFinder_Good); #else - bool sonar_ok = false; + bool rangefinder_ok = false; #endif // get position controller's target altitude above terrain @@ -251,8 +251,8 @@ float Copter::get_land_descent_speed() Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); } - // if we are above 10m and the sonar does not sense anything perform regular alt hold descent - if ((target_alt_cm >= LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { + // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent + if ((target_alt_cm >= LAND_START_ALT) && !(rangefinder_ok && rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (g.land_speed_high > 0) { // user has asked for a different landing speed than normal descent rate return -abs(g.land_speed_high); diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 3962fd6d34..99b3ba63f3 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -181,8 +181,8 @@ void Copter::loiter_run() attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // run altitude controller - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - // if sonar is ok, use surface tracking + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index efca852ee9..e4ce12fc24 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -520,8 +520,8 @@ void Copter::poshold_run() attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); // throttle control - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - // if sonar is ok, use surface tracking + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 8a8e49e013..3761183aa5 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -108,8 +108,8 @@ void Copter::sport_run() attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); // call throttle controller - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - // if sonar is ok, use surface tracking + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c9b300c9d6..78b48fcf1a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -41,7 +41,7 @@ enum aux_sw_func { AUXSW_SAVE_TRIM = 5, // save current position as level AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay - AUXSW_SONAR = 10, // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground + AUXSW_RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground AUXSW_FENCE = 11, // allow enabling or disabling fence in flight AUXSW_RESETTOARMEDYAW = 12, // changes yaw to be same as when quad was armed AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top @@ -148,7 +148,7 @@ enum tuning_func { TUNING_DECLINATION = 38, // compass declination in radians TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction) TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate - TUNING_SONAR_GAIN = 41, // sonar gain + TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain TUNING_EKF_VERTICAL_POS = 42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default TUNING_EKF_HORIZONTAL_POS = 43, // EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default TUNING_EKF_ACCEL_NOISE = 44, // EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level) diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 584212829d..607c64e9ea 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -44,10 +44,10 @@ void Copter::check_dynamic_flight(void) moving = (motors.get_throttle() > 0.8f || ahrs.pitch_sensor < -1500); } - if (!moving && sonar_enabled && sonar.status() == RangeFinder::RangeFinder_Good) { + if (!moving && rangefinder_enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) { // when we are more than 2m from the ground with good // rangefinder lock consider it to be dynamic flight - moving = (sonar.distance_cm() > 200); + moving = (rangefinder.distance_cm() > 200); } if (moving) { diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index eb938145df..377f3a5a8e 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -18,8 +18,8 @@ void Copter::update_precland() float final_alt = current_loc.alt; // use range finder altitude if it is valid - if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { - final_alt = sonar_alt; + if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { + final_alt = rangefinder_alt; } copter.precland.update(final_alt); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 49d5e8537d..c4cbc0378a 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -26,38 +26,38 @@ void Copter::read_barometer(void) motors.set_air_density_ratio(barometer.get_air_density_ratio()); } -#if RANGEFINDER_ENABLED == ENABLED -void Copter::init_sonar(void) +void Copter::init_rangefinder(void) { - sonar.init(); -} +#if RANGEFINDER_ENABLED == ENABLED + rangefinder.init(); #endif +} -// return sonar altitude in centimeters -int16_t Copter::read_sonar(void) +// return rangefinder altitude in centimeters +int16_t Copter::read_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED - sonar.update(); + rangefinder.update(); - // exit immediately if sonar is disabled - if (sonar.status() != RangeFinder::RangeFinder_Good) { - sonar_alt_health = 0; + // exit immediately if rangefinder is disabled + if (rangefinder.status() != RangeFinder::RangeFinder_Good) { + rangefinder_alt_health = 0; return 0; } - int16_t temp_alt = sonar.distance_cm(); + int16_t temp_alt = rangefinder.distance_cm(); - if (temp_alt >= sonar.min_distance_cm() && - temp_alt <= sonar.max_distance_cm() * SONAR_RELIABLE_DISTANCE_PCT) { - if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) { - sonar_alt_health++; + if (temp_alt >= rangefinder.min_distance_cm() && + temp_alt <= rangefinder.max_distance_cm() * RANGEFINDER_RELIABLE_DISTANCE_PCT) { + if (rangefinder_alt_health < RANGEFINDER_HEALTH_MAX) { + rangefinder_alt_health++; } }else{ - sonar_alt_health = 0; + rangefinder_alt_health = 0; } - #if SONAR_TILT_CORRECTION == 1 - // correct alt for angle of the sonar + #if RANGEFINDER_TILT_CORRECTION == 1 + // correct alt for angle of the rangefinder float temp = ahrs.cos_pitch() * ahrs.cos_roll(); temp = MAX(temp, 0.707f); temp_alt = (float)temp_alt * temp; diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index de733d47ac..989fb362f1 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -222,7 +222,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) // init channel options switch(ch_option) { case AUXSW_SIMPLE_MODE: - case AUXSW_SONAR: + case AUXSW_RANGEFINDER: case AUXSW_FENCE: case AUXSW_RESETTOARMEDYAW: case AUXSW_SUPERSIMPLE_MODE: @@ -345,13 +345,13 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif - case AUXSW_SONAR: - // enable or disable the sonar + case AUXSW_RANGEFINDER: + // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { - sonar_enabled = true; + rangefinder_enabled = true; }else{ - sonar_enabled = false; + rangefinder_enabled = false; } #endif break; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 97f8485698..a234e5dabe 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -256,10 +256,8 @@ void Copter::init_ardupilot() //----------------------------- init_barometer(true); - // initialise sonar -#if RANGEFINDER_ENABLED == ENABLED - init_sonar(); -#endif + // initialise rangefinder + init_rangefinder(); // initialise AP_RPM library rpm_sensor.init(); diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index b9cf9243d2..2a93d0ee3f 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -13,7 +13,7 @@ void Copter::terrain_update() #if RANGEFINDER_ENABLED == ENABLED float height; if (terrain.height_above_terrain(height, true)) { - sonar.set_estimated_terrain_height(height); + rangefinder.set_estimated_terrain_height(height); } #endif #endif diff --git a/ArduCopter/test.cpp b/ArduCopter/test.cpp index 1f5dab70a3..043d5915d3 100644 --- a/ArduCopter/test.cpp +++ b/ArduCopter/test.cpp @@ -20,7 +20,7 @@ static const struct Menu::command test_menu_commands[] = { {"shell", MENU_FUNC(test_shell)}, #endif #if HIL_MODE == HIL_MODE_DISABLED - {"rangefinder", MENU_FUNC(test_sonar)}, + {"rangefinder", MENU_FUNC(test_rangefinder)}, #endif }; @@ -243,21 +243,21 @@ int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv) /* * test the rangefinders */ -int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_rangefinder(uint8_t argc, const Menu::arg *argv) { #if RANGEFINDER_ENABLED == ENABLED - sonar.init(); + rangefinder.init(); - cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors()); + cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors()); print_hit_enter(); while(1) { delay(100); - sonar.update(); + rangefinder.update(); - cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm()); + cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm()); cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", - (int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1)); + (int)rangefinder._type[0], (int)rangefinder.status(0), rangefinder.distance_cm(0), (int)rangefinder._type[1], (int)rangefinder.status(1), rangefinder.distance_cm(1)); if(cliSerial->available() > 0) { return (0); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 6befdcb413..83745e2e5f 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -137,9 +137,9 @@ void Copter::tuning() { circle_nav.set_rate((float)g.rc_6.get_control_in()/25.0f-20.0f); break; - case TUNING_SONAR_GAIN: - // set sonar gain - g.sonar_gain.set(tuning_value); + case TUNING_RANGEFINDER_GAIN: + // set rangefinder gain + g.rangefinder_gain.set(tuning_value); break; #if 0