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

Copter: use only downward facing rangefinder

This commit is contained in:
Randy Mackay 2017-02-09 20:30:59 +09:00
parent bc89432019
commit 51c00f8144
7 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/AP_Arming.cpp
View File

@ -512,7 +512,8 @@ bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure)
// check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
// To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
if ((copter.rangefinder.num_sensors() > 0) && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm())) {
if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
return false;
}

2
ArduCopter/Copter.h
View File

@ -192,7 +192,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

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -211,13 +211,13 @@ void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan)
void NOINLINE Copter::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

4
ArduCopter/heli.cpp
View File

@ -42,10 +42,10 @@ void Copter::check_dynamic_flight(void)
moving = (motors->get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
}
if (!moving && rangefinder_state.enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) {
if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) {
// when we are more than 2m from the ground with good
// rangefinder lock consider it to be dynamic flight
moving = (rangefinder.distance_cm() > 200);
moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200);
}
if (moving) {

13
ArduCopter/sensors.cpp
View File

@ -29,7 +29,7 @@ void Copter::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
}
@ -43,10 +43,10 @@ void Copter::read_rangefinder(void)
should_log(MASK_LOG_CTUN)) {
DataFlash.Log_Write_RFND(rangefinder);
}
rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
int16_t temp_alt = rangefinder.distance_cm();
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_orient(ROTATION_PITCH_270);
#if RANGEFINDER_TILT_CORRECTION == ENABLED
// correct alt for angle of the rangefinder
@ -260,6 +260,7 @@ void Copter::init_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.init();
g2.proximity.set_rangefinder(&rangefinder);
#endif
}
@ -425,10 +426,10 @@ void Copter::update_sensor_status_flags(void)
#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;
}
}

2
ArduCopter/switches.cpp
View File

@ -307,7 +307,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
case AUXSW_RANGEFINDER:
// enable or disable the rangefinder
#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
ArduCopter/test.cpp
View File

@ -253,9 +253,12 @@ int8_t Copter::test_rangefinder(uint8_t argc, const Menu::arg *argv)
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);