Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-12 02:44:00 -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 #endif
#if RANGEFINDER_ENABLED == ENABLED #if RANGEFINDER_ENABLED == ENABLED
if (rangefinder.num_sensors() > 0) { if (rangefinder_state.enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
control_sensors_enabled |= 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; 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) void NOINLINE Sub::send_rangefinder(mavlink_channel_t chan)
{ {
// exit immediately if rangefinder is disabled // exit immediately if rangefinder is disabled
if (!rangefinder.has_data()) { if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
return; return;
} }
mavlink_msg_rangefinder_send( mavlink_msg_rangefinder_send(
chan, chan,
rangefinder.distance_cm() * 0.01f, rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
rangefinder.voltage_mv() * 0.001f); rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
} }
#endif #endif

2
ArduSub/Sub.h
View File

@ -176,7 +176,7 @@ private:
Compass compass; Compass compass;
AP_InertialSensor ins; AP_InertialSensor ins;
RangeFinder rangefinder {serial_manager}; RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
struct { struct {
bool enabled:1; bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder 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() bool Sub::auto_terrain_recover_start()
{ {
// Check rangefinder status to see if recovery is possible // 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_OutOfRangeLow:
case RangeFinder::RangeFinder_OutOfRangeHigh: case RangeFinder::RangeFinder_OutOfRangeHigh:
@ -720,7 +720,7 @@ void Sub::auto_terrain_recover_run()
return; return;
} }
switch (rangefinder.status()) { switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::RangeFinder_OutOfRangeLow: case RangeFinder::RangeFinder_OutOfRangeLow:
target_climb_rate = wp_nav.get_speed_up(); 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 #if RANGEFINDER_ENABLED == ENABLED
rangefinder.init(); rangefinder.init();
rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); 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 #endif
} }
@ -35,9 +35,9 @@ void Sub::read_rangefinder(void)
#if RANGEFINDER_ENABLED == ENABLED #if RANGEFINDER_ENABLED == ENABLED
rangefinder.update(); 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 #if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder // 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: case AUXSW_RANGEFINDER:
// enable or disable the sonar // enable or disable the sonar
#if RANGEFINDER_ENABLED == ENABLED #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; rangefinder_state.enabled = true;
} else { } else {
rangefinder_state.enabled = false; 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); hal.scheduler->delay(100);
rangefinder.update(); rangefinder.update();
cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm()); for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", cliSerial->printf("Dev%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)); (int)i,
(int)rangefinder.status(i),
(int)rangefinder.distance_cm(i));
}
if (cliSerial->available() > 0) { if (cliSerial->available() > 0) {
return (0); return (0);