diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index ab2d8e25b2..32d9d72078 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -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; } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 82fd880f73..a24dcc0ece 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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 diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 39589a3702..ff92f30c2e 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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 diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 3e0e70c0aa..9b33c8afec 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -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) { diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 3cfc2f5a29..703f5da68d 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -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; } } diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index ba0a2590f7..f97b47d65c 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -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; diff --git a/ArduCopter/test.cpp b/ArduCopter/test.cpp index 31db5b9ba6..784d27f8b0 100644 --- a/ArduCopter/test.cpp +++ b/ArduCopter/test.cpp @@ -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; iprintf("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);