Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: use only downward facing rangefinder

Browse Source
This commit is contained in:
Randy Mackay 2017-02-27 11:32:58 +09:00
parent f29fe8bb88
commit 6798162a98
6 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduSub/GCS_Mavlink.cpp
View File

@ -224,10 +224,10 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
#endif
#if RANGEFINDER_ENABLED == ENABLED
if (rangefinder.num_sensors() > 0) {
if (rangefinder_state.enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (rangefinder.has_data()) {
if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
@ -379,13 +379,13 @@ void NOINLINE Sub::send_current_waypoint(mavlink_channel_t chan)
void NOINLINE Sub::send_rangefinder(mavlink_channel_t chan)
{
// exit immediately if rangefinder is disabled
if (!rangefinder.has_data()) {
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
return;
}
mavlink_msg_rangefinder_send(
chan,
rangefinder.distance_cm() * 0.01f,
rangefinder.voltage_mv() * 0.001f);
rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
}
#endif

2
ArduSub/Sub.h
View File

@ -176,7 +176,7 @@ private:
Compass compass;
AP_InertialSensor ins;
RangeFinder rangefinder {serial_manager};
RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
struct {
bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder

4
ArduSub/control_auto.cpp
View File

@ -665,7 +665,7 @@ float Sub::get_auto_heading(void)
bool Sub::auto_terrain_recover_start()
{
// Check rangefinder status to see if recovery is possible
switch (rangefinder.status()) {
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::RangeFinder_OutOfRangeLow:
case RangeFinder::RangeFinder_OutOfRangeHigh:
@ -720,7 +720,7 @@ void Sub::auto_terrain_recover_run()
return;
}
switch (rangefinder.status()) {
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::RangeFinder_OutOfRangeLow:
target_climb_rate = wp_nav.get_speed_up();

6
ArduSub/sensors.cpp
View File

@ -25,7 +25,7 @@ void Sub::init_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.init();
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
rangefinder_state.enabled = (rangefinder.num_sensors() >= 1);
rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
#endif
}
@ -35,9 +35,9 @@ void Sub::read_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED
rangefinder.update();
rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
int16_t temp_alt = rangefinder.distance_cm();
int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
#if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder

2
ArduSub/switches.cpp
View File

@ -209,7 +209,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
case AUXSW_RANGEFINDER:
// enable or disable the sonar
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
rangefinder_state.enabled = true;
} else {
rangefinder_state.enabled = false;

9
ArduSub/test.cpp
View File

@ -253,9 +253,12 @@ int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
hal.scheduler->delay(100);
rangefinder.update();
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)rangefinder._type[0], (int)rangefinder.status(0), rangefinder.distance_cm(0), (int)rangefinder._type[1], (int)rangefinder.status(1), rangefinder.distance_cm(1));
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
cliSerial->printf("Dev%d: status %d distance_cm %d\n",
(int)i,
(int)rangefinder.status(i),
(int)rangefinder.distance_cm(i));
}
if (cliSerial->available() > 0) {
return (0);