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

Rover: rename sonar to rangefinder

This commit is contained in:
khancyr 2017-07-13 13:36:44 +02:00 committed by Francisco Ferreira
parent 42181ee7c8
commit 40b860e240
11 changed files with 138 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.cpp
View File

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

36
APMrover2/GCS_Mavlink.cpp
View File

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

42
APMrover2/Log.cpp
View File

@ -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<int8_t>(obstacle.turn_angle),
turn_time : turn_time,
ground_speed : static_cast<uint16_t>(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<int8_t>(obstacle.turn_angle),
turn_time : turn_time,
ground_speed : static_cast<uint16_t>(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) {}

16
APMrover2/Parameters.cpp
View File

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

18
APMrover2/Parameters.h
View File

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

2
APMrover2/Rover.cpp
View File

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

20
APMrover2/Rover.h
View File

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

4
APMrover2/defines.h
View File

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

72
APMrover2/sensors.cpp
View File

@ -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<uint16_t>(g.sonar_trigger_cm) &&
obstacle.sonar1_distance_cm < static_cast<uint16_t>(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<uint16_t>(g.rangefinder_trigger_cm) &&
obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(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<uint32_t>(obstacle.sonar1_distance_cm));
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder1 obstacle %u cm",
static_cast<uint32_t>(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<uint16_t>(g.sonar_trigger_cm)) {
obstacle.turn_angle = g.rangefinder_turn_angle;
} else if (obstacle.rangefinder2_distance_cm < static_cast<uint16_t>(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<uint32_t>(obstacle.sonar2_distance_cm));
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder2 obstacle %u cm",
static_cast<uint32_t>(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<uint16_t>(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<uint16_t>(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<uint32_t>(obstacle.sonar1_distance_cm));
if (obstacle.detected_count == g.rangefinder_debounce) {
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder obstacle %u cm",
static_cast<uint32_t>(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;
}
}

4
APMrover2/system.cpp
View File

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

60
APMrover2/test.cpp
View File

@ -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<double>(sonar_dist_cm_min),
static_cast<double>(sonar_dist_cm_max),
cliSerial->printf("rangefinder1 dist=%.1f:%.1fcm volt1=%.2f:%.2f rangefinder2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n",
static_cast<double>(rangefinder_dist_cm_min),
static_cast<double>(rangefinder_dist_cm_max),
static_cast<double>(voltage_min),
static_cast<double>(voltage_max),
static_cast<double>(sonar2_dist_cm_min),
static_cast<double>(sonar2_dist_cm_max),
static_cast<double>(rangefinder2_dist_cm_min),
static_cast<double>(rangefinder2_dist_cm_max),
static_cast<double>(voltage2_min),
static_cast<double>(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) {