From 40b860e2403d3206f385bab8cb4f94f2ff393a14 Mon Sep 17 00:00:00 2001 From: khancyr Date: Thu, 13 Jul 2017 13:36:44 +0200 Subject: [PATCH] Rover: rename sonar to rangefinder --- APMrover2/APMrover2.cpp | 2 +- APMrover2/GCS_Mavlink.cpp | 36 ++++++++++---------- APMrover2/Log.cpp | 42 +++++++++++------------ APMrover2/Parameters.cpp | 16 ++++----- APMrover2/Parameters.h | 18 +++++----- APMrover2/Rover.cpp | 2 +- APMrover2/Rover.h | 20 +++++------ APMrover2/defines.h | 4 +-- APMrover2/sensors.cpp | 72 +++++++++++++++++++-------------------- APMrover2/system.cpp | 4 +-- APMrover2/test.cpp | 60 ++++++++++++++++---------------- 11 files changed, 138 insertions(+), 138 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 256e243a67..2422f596db 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -46,7 +46,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { // Function name, Hz, us, SCHED_TASK(read_radio, 50, 1000), SCHED_TASK(ahrs_update, 50, 6400), - SCHED_TASK(read_sonars, 50, 2000), + SCHED_TASK(read_rangefinders, 50, 2000), SCHED_TASK(update_current_mode, 50, 1500), SCHED_TASK(set_servos, 50, 1500), SCHED_TASK(update_GPS_50Hz, 50, 2500), diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 85483f7fa8..9db2b8544d 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -202,8 +202,8 @@ void Rover::send_hwstatus(mavlink_channel_t chan) void Rover::send_rangefinder(mavlink_channel_t chan) { - if (!sonar.has_data(0) && !sonar.has_data(1)) { - // no sonar to report + if (!rangefinder.has_data(0) && !rangefinder.has_data(1)) { + // no rangefinder to report return; } @@ -211,25 +211,25 @@ void Rover::send_rangefinder(mavlink_channel_t chan) float voltage = 0.0f; /* - report smaller distance of two sonars + report smaller distance of two rangefinders */ - if (sonar.has_data(0) && sonar.has_data(1)) { - if (sonar.distance_cm(0) <= sonar.distance_cm(1)) { - distance_cm = sonar.distance_cm(0); - voltage = sonar.voltage_mv(0); + if (rangefinder.has_data(0) && rangefinder.has_data(1)) { + if (rangefinder.distance_cm(0) <= rangefinder.distance_cm(1)) { + distance_cm = rangefinder.distance_cm(0); + voltage = rangefinder.voltage_mv(0); } else { - distance_cm = sonar.distance_cm(1); - voltage = sonar.voltage_mv(1); + distance_cm = rangefinder.distance_cm(1); + voltage = rangefinder.voltage_mv(1); } } else { - // only sonar 0 or sonar 1 has data - if (sonar.has_data(0)) { - distance_cm = sonar.distance_cm(0); - voltage = sonar.voltage_mv(0) * 0.001f; + // only rangefinder 0 or rangefinder 1 has data + if (rangefinder.has_data(0)) { + distance_cm = rangefinder.distance_cm(0); + voltage = rangefinder.voltage_mv(0) * 0.001f; } - if (sonar.has_data(1)) { - distance_cm = sonar.distance_cm(1); - voltage = sonar.voltage_mv(1) * 0.001f; + if (rangefinder.has_data(1)) { + distance_cm = rangefinder.distance_cm(1); + voltage = rangefinder.voltage_mv(1) * 0.001f; } } @@ -426,7 +426,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); rover.send_rangefinder(chan); - send_distance_sensor(rover.sonar); + send_distance_sensor(rover.rangefinder); break; case MSG_MOUNT_STATUS: @@ -1454,7 +1454,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: - rover.sonar.handle_msg(msg); + rover.rangefinder.handle_msg(msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 164278fe10..0f527dd9c4 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -43,7 +43,7 @@ bool Rover::print_log_menu(void) PLOG(IMU); PLOG(CMD); PLOG(CURRENT); - PLOG(SONAR); + PLOG(RANGEFINDER); PLOG(COMPASS); PLOG(CAMERA); PLOG(STEERING); @@ -120,7 +120,7 @@ int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv) TARG(IMU); TARG(CMD); TARG(CURRENT); - TARG(SONAR); + TARG(RANGEFINDER); TARG(COMPASS); TARG(CAMERA); TARG(STEERING); @@ -306,12 +306,12 @@ void Rover::Log_Write_Attitude() DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pidSpeedThrottle.get_pid_info()); } -struct PACKED log_Sonar { +struct PACKED log_Rangefinder { LOG_PACKET_HEADER; uint64_t time_us; float lateral_accel; - uint16_t sonar1_distance; - uint16_t sonar2_distance; + uint16_t rangefinder1_distance; + uint16_t rangefinder2_distance; uint16_t detected_count; int8_t turn_angle; uint16_t turn_time; @@ -319,24 +319,24 @@ struct PACKED log_Sonar { int8_t throttle; }; -// Write a sonar packet -void Rover::Log_Write_Sonar() +// Write a rangefinder packet +void Rover::Log_Write_Rangefinder() { uint16_t turn_time = 0; if (!is_zero(obstacle.turn_angle)) { turn_time = AP_HAL::millis() - obstacle.detected_time_ms; } - struct log_Sonar pkt = { - LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), - time_us : AP_HAL::micros64(), - lateral_accel : lateral_acceleration, - sonar1_distance : sonar.distance_cm(0), - sonar2_distance : sonar.distance_cm(1), - detected_count : obstacle.detected_count, - turn_angle : static_cast(obstacle.turn_angle), - turn_time : turn_time, - ground_speed : static_cast(fabsf(ground_speed * 100)), - throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) + struct log_Rangefinder pkt = { + LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG), + time_us : AP_HAL::micros64(), + lateral_accel : lateral_acceleration, + rangefinder1_distance : rangefinder.distance_cm(0), + rangefinder2_distance : rangefinder.distance_cm(1), + detected_count : obstacle.detected_count, + turn_angle : static_cast(obstacle.turn_angle), + turn_time : turn_time, + ground_speed : static_cast(fabsf(ground_speed * 100)), + throttle : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -484,8 +484,8 @@ const LogStructure Rover::log_structure[] = { "CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY" }, { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), "NTUN", "QHfHHbf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT" }, - { LOG_SONAR_MSG, sizeof(log_Sonar), - "SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, + { LOG_RANGEFINDER_MSG, sizeof(log_Rangefinder), + "RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr" }, { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), "ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, { LOG_STEERING_MSG, sizeof(log_Steering), @@ -551,7 +551,7 @@ void Rover::Log_Write_Nav_Tuning() {} void Rover::Log_Write_Performance() {} int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } void Rover::Log_Write_Control_Tuning() {} -void Rover::Log_Write_Sonar() {} +void Rover::Log_Write_Rangefinder() {} void Rover::Log_Write_Attitude() {} void Rover::start_logging() {} void Rover::Log_Write_RC(void) {} diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 1583baea68..256b986166 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -25,9 +25,9 @@ const AP_Param::Info Rover::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask - // @Description: Bitmap of what log types to enable in dataflash. This values is made up of the sum of each of the log types you want to be saved on dataflash. On a PX4 or Pixhawk the large storage size of a microSD card means it is usually best just to enable all log types by setting this to 65535. On APM2 the smaller 4 MByte dataflash means you need to be more selective in your logging or you may run out of log space while flying (in which case it will wrap and overwrite the start of the log). The individual bits are ATTITUDE_FAST=1, ATTITUDE_MEDIUM=2, GPS=4, PerformanceMonitoring=8, ControlTuning=16, NavigationTuning=32, Mode=64, IMU=128, Commands=256, Battery=512, Compass=1024, TECS=2048, Camera=4096, RCandServo=8192, Sonar=16384, Arming=32768, FullLogs=65535 + // @Description: Bitmap of what log types to enable in dataflash. This values is made up of the sum of each of the log types you want to be saved on dataflash. On a PX4 or Pixhawk the large storage size of a microSD card means it is usually best just to enable all log types by setting this to 65535. On APM2 the smaller 4 MByte dataflash means you need to be more selective in your logging or you may run out of log space while flying (in which case it will wrap and overwrite the start of the log). The individual bits are ATTITUDE_FAST=1, ATTITUDE_MEDIUM=2, GPS=4, PerformanceMonitoring=8, ControlTuning=16, NavigationTuning=32, Mode=64, IMU=128, Commands=256, Battery=512, Compass=1024, TECS=2048, Camera=4096, RCandServo=8192, Rangefinder=16384, Arming=32768, FullLogs=65535 // @Values: 0:Disabled,5190:APM2-Default,65535:PX4/Pixhawk-Default - // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:MODE,7:IMU,8:CMD,9:CURRENT,10:COMPASS,11:TECS,12:CAMERA,13:RC,14:SONAR,15:ARM/DISARM,19:IMU_RAW + // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:MODE,7:IMU,8:CMD,9:CURRENT,10:COMPASS,11:TECS,12:CAMERA,13:RC,14:RANGEFINDER,15:ARM/DISARM,19:IMU_RAW // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -259,7 +259,7 @@ const AP_Param::Info Rover::var_info[] = { // @Range: 0 1000 // @Increment: 1 // @User: Standard - GSCALAR(sonar_trigger_cm, "RNGFND_TRIGGR_CM", 100), + GSCALAR(rangefinder_trigger_cm, "RNGFND_TRIGGR_CM", 100), // @Param: RNGFND_TURN_ANGL // @DisplayName: Rangefinder trigger angle @@ -268,7 +268,7 @@ const AP_Param::Info Rover::var_info[] = { // @Range: -45 45 // @Increment: 1 // @User: Standard - GSCALAR(sonar_turn_angle, "RNGFND_TURN_ANGL", 45), + GSCALAR(rangefinder_turn_angle, "RNGFND_TURN_ANGL", 45), // @Param: RNGFND_TURN_TIME // @DisplayName: Rangefinder turn time @@ -277,15 +277,15 @@ const AP_Param::Info Rover::var_info[] = { // @Range: 0 100 // @Increment: 0.1 // @User: Standard - GSCALAR(sonar_turn_time, "RNGFND_TURN_TIME", 1.0f), + GSCALAR(rangefinder_turn_time, "RNGFND_TURN_TIME", 1.0f), // @Param: RNGFND_DEBOUNCE // @DisplayName: Rangefinder debounce count - // @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle. + // @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false rangefinder events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle. // @Range: 1 100 // @Increment: 1 // @User: Standard - GSCALAR(sonar_debounce, "RNGFND_DEBOUNCE", 2), + GSCALAR(rangefinder_debounce, "RNGFND_DEBOUNCE", 2), // @Param: LEARN_CH // @DisplayName: Learning channel @@ -417,7 +417,7 @@ const AP_Param::Info Rover::var_info[] = { // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp - GOBJECT(sonar, "RNGFND", RangeFinder), + GOBJECT(rangefinder, "RNGFND", RangeFinder), // @Group: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 5308466e52..c947390cff 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -143,12 +143,12 @@ public: // obstacle control k_param_sonar_enabled = 190, // deprecated, can be removed k_param_sonar_old, // unused - k_param_sonar_trigger_cm, - k_param_sonar_turn_angle, - k_param_sonar_turn_time, + k_param_rangefinder_trigger_cm, + k_param_rangefinder_turn_angle, + k_param_rangefinder_turn_time, k_param_sonar2_old, // unused - k_param_sonar_debounce, - k_param_sonar, // sonar object + k_param_rangefinder_debounce, + k_param_rangefinder, // rangefinder object // // 210: driving modes @@ -259,10 +259,10 @@ public: AP_Int8 fs_crash_check; // obstacle control - AP_Int16 sonar_trigger_cm; - AP_Float sonar_turn_angle; - AP_Float sonar_turn_time; - AP_Int8 sonar_debounce; + AP_Int16 rangefinder_trigger_cm; + AP_Float rangefinder_turn_angle; + AP_Float rangefinder_turn_time; + AP_Int8 rangefinder_debounce; // driving modes diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 01a3246888..17de33b35e 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -43,7 +43,7 @@ Rover::Rover(void) : control_mode(INITIALISING), throttle(500), #if FRSKY_TELEM_ENABLED == ENABLED - frsky_telemetry(ahrs, battery, sonar), + frsky_telemetry(ahrs, battery, rangefinder), #endif do_auto_rotation(false), home(ahrs.get_home()), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6836f5151c..7ed08b0f5a 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -151,7 +151,7 @@ private: AP_Baro barometer; Compass compass; AP_InertialSensor ins; - RangeFinder sonar { serial_manager, ROTATION_NONE }; + RangeFinder rangefinder { serial_manager, ROTATION_NONE }; AP_Button button; // flight modes convenience array @@ -159,9 +159,9 @@ private: // Inertial Navigation EKF #if AP_AHRS_NAVEKF_AVAILABLE - NavEKF2 EKF2{&ahrs, barometer, sonar}; - NavEKF3 EKF3{&ahrs, barometer, sonar}; - AP_AHRS_NavEKF ahrs {ins, barometer, gps, sonar, EKF2, EKF3}; + NavEKF2 EKF2{&ahrs, barometer, rangefinder}; + NavEKF3 EKF3{&ahrs, barometer, rangefinder}; + AP_AHRS_NavEKF ahrs {ins, barometer, gps, rangefinder, EKF2, EKF3}; #else AP_AHRS_DCM ahrs {ins, barometer, gps}; #endif @@ -277,8 +277,8 @@ private: // have we detected an obstacle? uint8_t detected_count; float turn_angle; - uint16_t sonar1_distance_cm; - uint16_t sonar2_distance_cm; + uint16_t rangefinder1_distance_cm; + uint16_t rangefinder2_distance_cm; // time when we last detected an obstacle, in milliseconds uint32_t detected_time_ms; @@ -461,7 +461,7 @@ private: void Log_Write_Startup(uint8_t type); void Log_Write_Control_Tuning(); void Log_Write_Nav_Tuning(); - void Log_Write_Sonar(); + void Log_Write_Rangefinder(); void Log_Write_Beacon(); void Log_Write_Current(); void Log_Write_Attitude(); @@ -522,7 +522,7 @@ private: void trim_control_surfaces(); void trim_radio(); void init_barometer(bool full_calibration); - void init_sonar(void); + void init_rangefinder(void); void init_beacon(); void update_beacon(); void init_visual_odom(); @@ -530,7 +530,7 @@ private: void update_wheel_encoder(); void read_battery(void); void read_receiver_rssi(void); - void read_sonars(void); + void read_rangefinders(void); void report_batt_monitor(); void report_radio(); void report_gains(); @@ -623,7 +623,7 @@ public: int8_t test_gps(uint8_t argc, const Menu::arg *argv); int8_t test_ins(uint8_t argc, const Menu::arg *argv); int8_t test_mag(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); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN int8_t test_shell(uint8_t argc, const Menu::arg *argv); #endif diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 9643f09408..aa2cb20e10 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -56,7 +56,7 @@ enum GuidedMode { #define LOG_NTUN_MSG 0x02 #define LOG_PERFORMANCE_MSG 0x03 #define LOG_STARTUP_MSG 0x06 -#define LOG_SONAR_MSG 0x07 +#define LOG_RANGEFINDER_MSG 0x07 #define LOG_ARM_DISARM_MSG 0x08 #define LOG_STEERING_MSG 0x0D #define LOG_GUIDEDTARGET_MSG 0x0E @@ -77,7 +77,7 @@ enum GuidedMode { #define MASK_LOG_IMU (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CURRENT (1<<9) -#define MASK_LOG_SONAR (1<<10) +#define MASK_LOG_RANGEFINDER (1<<10) #define MASK_LOG_COMPASS (1<<11) #define MASK_LOG_CAMERA (1<<12) #define MASK_LOG_STEERING (1<<13) diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index d6442bde81..8173bd79cc 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -48,9 +48,9 @@ void Rover::init_barometer(bool full_calibration) gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete"); } -void Rover::init_sonar(void) +void Rover::init_rangefinder(void) { - sonar.init(); + rangefinder.init(); } // init beacons used for non-gps position estimates @@ -133,67 +133,67 @@ void Rover::accel_cal_update() { } } -// read the sonars -void Rover::read_sonars(void) +// read the rangefinders +void Rover::read_rangefinders(void) { - sonar.update(); + rangefinder.update(); - if (sonar.status(0) == RangeFinder::RangeFinder_NotConnected) { - // this makes it possible to disable sonar at runtime + if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected) { + // this makes it possible to disable rangefinder at runtime return; } - if (sonar.has_data(1)) { - // we have two sonars - obstacle.sonar1_distance_cm = sonar.distance_cm(0); - obstacle.sonar2_distance_cm = sonar.distance_cm(1); - if (obstacle.sonar1_distance_cm < static_cast(g.sonar_trigger_cm) && - obstacle.sonar1_distance_cm < static_cast(obstacle.sonar2_distance_cm)) { + if (rangefinder.has_data(1)) { + // we have two rangefinders + obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0); + obstacle.rangefinder2_distance_cm = rangefinder.distance_cm(1); + if (obstacle.rangefinder1_distance_cm < static_cast(g.rangefinder_trigger_cm) && + obstacle.rangefinder1_distance_cm < static_cast(obstacle.rangefinder2_distance_cm)) { // we have an object on the left if (obstacle.detected_count < 127) { obstacle.detected_count++; } - if (obstacle.detected_count == g.sonar_debounce) { - gcs().send_text(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm", - static_cast(obstacle.sonar1_distance_cm)); + if (obstacle.detected_count == g.rangefinder_debounce) { + gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder1 obstacle %u cm", + static_cast(obstacle.rangefinder1_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); - obstacle.turn_angle = g.sonar_turn_angle; - } else if (obstacle.sonar2_distance_cm < static_cast(g.sonar_trigger_cm)) { + obstacle.turn_angle = g.rangefinder_turn_angle; + } else if (obstacle.rangefinder2_distance_cm < static_cast(g.rangefinder_trigger_cm)) { // we have an object on the right if (obstacle.detected_count < 127) { obstacle.detected_count++; } - if (obstacle.detected_count == g.sonar_debounce) { - gcs().send_text(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm", - static_cast(obstacle.sonar2_distance_cm)); + if (obstacle.detected_count == g.rangefinder_debounce) { + gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder2 obstacle %u cm", + static_cast(obstacle.rangefinder2_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); - obstacle.turn_angle = -g.sonar_turn_angle; + obstacle.turn_angle = -g.rangefinder_turn_angle; } } else { - // we have a single sonar - obstacle.sonar1_distance_cm = sonar.distance_cm(0); - obstacle.sonar2_distance_cm = 0; - if (obstacle.sonar1_distance_cm < static_cast(g.sonar_trigger_cm)) { + // we have a single rangefinder + obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0); + obstacle.rangefinder2_distance_cm = 0; + if (obstacle.rangefinder1_distance_cm < static_cast(g.rangefinder_trigger_cm)) { // obstacle detected in front if (obstacle.detected_count < 127) { obstacle.detected_count++; } - if (obstacle.detected_count == g.sonar_debounce) { - gcs().send_text(MAV_SEVERITY_INFO, "Sonar obstacle %u cm", - static_cast(obstacle.sonar1_distance_cm)); + if (obstacle.detected_count == g.rangefinder_debounce) { + gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder obstacle %u cm", + static_cast(obstacle.rangefinder1_distance_cm)); } obstacle.detected_time_ms = AP_HAL::millis(); - obstacle.turn_angle = g.sonar_turn_angle; + obstacle.turn_angle = g.rangefinder_turn_angle; } } - Log_Write_Sonar(); + Log_Write_Rangefinder(); // no object detected - reset after the turn time - if (obstacle.detected_count >= g.sonar_debounce && - AP_HAL::millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { + if (obstacle.detected_count >= g.rangefinder_debounce && + AP_HAL::millis() > obstacle.detected_time_ms + g.rangefinder_turn_time*1000) { gcs().send_text(MAV_SEVERITY_INFO, "Obstacle passed"); obstacle.detected_count = 0; obstacle.turn_angle = 0; @@ -296,12 +296,12 @@ void Rover::update_sensor_status_flags(void) control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } - if (sonar.num_sensors() > 0) { + if (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (g.sonar_trigger_cm > 0) { + if (g.rangefinder_trigger_cm > 0) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } - if (sonar.has_data(0)) { + if (rangefinder.has_data(0)) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 350763e49d..4a151cf81b 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -156,8 +156,8 @@ void Rover::init_ardupilot() // initialise compass init_compass(); - // initialise sonar - init_sonar(); + // initialise rangefinder + init_rangefinder(); // init beacons used for non-gps position estimation init_beacon(); diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index f6cc92d2ad..e706fa31a5 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -17,7 +17,7 @@ static const struct Menu::command test_menu_commands[] = { // when real sensors are attached or they are emulated {"gps", MENU_FUNC(test_gps)}, {"ins", MENU_FUNC(test_ins)}, - {"sonartest", MENU_FUNC(test_sonar)}, + {"rngfndtest", MENU_FUNC(test_rangefinder)}, {"compass", MENU_FUNC(test_mag)}, {"logging", MENU_FUNC(test_logging)}, #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN @@ -359,67 +359,67 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) //------------------------------------------------------------------------------------------- // real sensors that have not been simulated yet go here -int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) +int8_t Rover::test_rangefinder(uint8_t argc, const Menu::arg *argv) { - init_sonar(); + init_rangefinder(); delay(20); - sonar.update(); + rangefinder.update(); - if (sonar.status(0) == RangeFinder::RangeFinder_NotConnected && sonar.status(1) == RangeFinder::RangeFinder_NotConnected) { - cliSerial->printf("WARNING: Sonar is not enabled\n"); + if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected && rangefinder.status(1) == RangeFinder::RangeFinder_NotConnected) { + cliSerial->printf("WARNING: Rangefinder is not enabled\n"); } print_hit_enter(); - float sonar_dist_cm_min = 0.0f; - float sonar_dist_cm_max = 0.0f; + float rangefinder_dist_cm_min = 0.0f; + float rangefinder_dist_cm_max = 0.0f; float voltage_min = 0.0f, voltage_max = 0.0f; - float sonar2_dist_cm_min = 0.0f; - float sonar2_dist_cm_max = 0.0f; + float rangefinder2_dist_cm_min = 0.0f; + float rangefinder2_dist_cm_max = 0.0f; float voltage2_min = 0.0f, voltage2_max = 0.0f; uint32_t last_print = 0; while (true) { delay(20); - sonar.update(); + rangefinder.update(); uint32_t now = millis(); - float dist_cm = sonar.distance_cm(0); - float voltage = sonar.voltage_mv(0); - if (is_zero(sonar_dist_cm_min)) { - sonar_dist_cm_min = dist_cm; + float dist_cm = rangefinder.distance_cm(0); + float voltage = rangefinder.voltage_mv(0); + if (is_zero(rangefinder_dist_cm_min)) { + rangefinder_dist_cm_min = dist_cm; voltage_min = voltage; } - sonar_dist_cm_max = MAX(sonar_dist_cm_max, dist_cm); - sonar_dist_cm_min = MIN(sonar_dist_cm_min, dist_cm); + rangefinder_dist_cm_max = MAX(rangefinder_dist_cm_max, dist_cm); + rangefinder_dist_cm_min = MIN(rangefinder_dist_cm_min, dist_cm); voltage_min = MIN(voltage_min, voltage); voltage_max = MAX(voltage_max, voltage); - dist_cm = sonar.distance_cm(1); - voltage = sonar.voltage_mv(1); - if (is_zero(sonar2_dist_cm_min)) { - sonar2_dist_cm_min = dist_cm; + dist_cm = rangefinder.distance_cm(1); + voltage = rangefinder.voltage_mv(1); + if (is_zero(rangefinder2_dist_cm_min)) { + rangefinder2_dist_cm_min = dist_cm; voltage2_min = voltage; } - sonar2_dist_cm_max = MAX(sonar2_dist_cm_max, dist_cm); - sonar2_dist_cm_min = MIN(sonar2_dist_cm_min, dist_cm); + rangefinder2_dist_cm_max = MAX(rangefinder2_dist_cm_max, dist_cm); + rangefinder2_dist_cm_min = MIN(rangefinder2_dist_cm_min, dist_cm); voltage2_min = MIN(voltage2_min, voltage); voltage2_max = MAX(voltage2_max, voltage); if (now - last_print >= 200) { - cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", - static_cast(sonar_dist_cm_min), - static_cast(sonar_dist_cm_max), + cliSerial->printf("rangefinder1 dist=%.1f:%.1fcm volt1=%.2f:%.2f rangefinder2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", + static_cast(rangefinder_dist_cm_min), + static_cast(rangefinder_dist_cm_max), static_cast(voltage_min), static_cast(voltage_max), - static_cast(sonar2_dist_cm_min), - static_cast(sonar2_dist_cm_max), + static_cast(rangefinder2_dist_cm_min), + static_cast(rangefinder2_dist_cm_max), static_cast(voltage2_min), static_cast(voltage2_max)); voltage_min = voltage_max = 0.0f; voltage2_min = voltage2_max = 0.0f; - sonar_dist_cm_min = sonar_dist_cm_max = 0.0f; - sonar2_dist_cm_min = sonar2_dist_cm_max = 0.0f; + rangefinder_dist_cm_min = rangefinder_dist_cm_max = 0.0f; + rangefinder2_dist_cm_min = rangefinder2_dist_cm_max = 0.0f; last_print = now; } if (cliSerial->available() > 0) {