Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: rename sonar to rangefinder

This commit is contained in:
Randy Mackay 2016-04-27 20:37:04 +09:00
parent 028946ae9e
commit 7689315ba2
25 changed files with 116 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.cpp
View File

@ -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)) {

18
ArduCopter/Attitude.cpp
View File

@ -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);
}

8
ArduCopter/Copter.cpp
View File

@ -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),

23
ArduCopter/Copter.h
View File

@ -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);
};

14
ArduCopter/GCS_Mavlink.cpp
View File

@ -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;
}

8
ArduCopter/Log.cpp
View File

@ -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

8
ArduCopter/Parameters.cpp
View File

@ -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

10
ArduCopter/Parameters.h
View File

@ -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

2
ArduCopter/arming_checks.cpp
View File

@ -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");
}

24
ArduCopter/config.h
View File

@ -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

4
ArduCopter/control_althold.cpp
View File

@ -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);
}

4
ArduCopter/control_circle.cpp
View File

@ -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

8
ArduCopter/control_land.cpp
View File

@ -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);

4
ArduCopter/control_loiter.cpp
View File

@ -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);
}

4
ArduCopter/control_poshold.cpp
View File

@ -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

4
ArduCopter/control_sport.cpp
View File

@ -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);
}

4
ArduCopter/defines.h
View File

@ -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)

4
ArduCopter/heli.cpp
View File

@ -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) {

4
ArduCopter/precision_landing.cpp
View File

@ -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);

36
ArduCopter/sensors.cpp
View File

@ -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;

10
ArduCopter/switches.cpp
View File

@ -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;

6
ArduCopter/system.cpp
View File

@ -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();

2
ArduCopter/terrain.cpp
View File

@ -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

14
ArduCopter/test.cpp
View File

@ -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);

6
ArduCopter/tuning.cpp
View File

@ -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