Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-20 15:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: use only downward facing rangefinder

Browse Source
This commit is contained in:
Randy Mackay 2017-02-09 20:39:58 +09:00
parent c100f53ee6
commit c0f155fd27
5 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/GCS_Mavlink.cpp
View File

@ -392,14 +392,14 @@ void Plane::send_pid_tuning(mavlink_channel_t chan)
void Plane::send_rangefinder(mavlink_channel_t chan) void Plane::send_rangefinder(mavlink_channel_t chan)
{ {
if (!rangefinder.has_data()) { if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
// no sonar to report // no sonar to report
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);
} }
void Plane::send_current_waypoint(mavlink_channel_t chan) void Plane::send_current_waypoint(mavlink_channel_t chan)

6
ArduPlane/Log.cpp
View File

@ -343,15 +343,15 @@ struct PACKED log_Sonar {
void Plane::Log_Write_Sonar() void Plane::Log_Write_Sonar()
{ {
uint16_t distance = 0; uint16_t distance = 0;
if (rangefinder.status() == RangeFinder::RangeFinder_Good) { if (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) {
distance = rangefinder.distance_cm(); distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
} }
struct log_Sonar pkt = { struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
distance : (float)distance*0.01f, distance : (float)distance*0.01f,
voltage : rangefinder.voltage_mv()*0.001f, voltage : rangefinder.voltage_mv_orient(ROTATION_PITCH_270)*0.001f,
count : rangefinder_state.in_range_count, count : rangefinder_state.in_range_count,
correction : rangefinder_state.correction correction : rangefinder_state.correction
}; };

2
ArduPlane/Plane.h
View File

@ -200,7 +200,7 @@ private:
AP_InertialSensor ins; AP_InertialSensor ins;
RangeFinder rangefinder {serial_manager}; RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
AP_Vehicle::FixedWing::Rangefinder_State rangefinder_state; AP_Vehicle::FixedWing::Rangefinder_State rangefinder_state;

8
ArduPlane/altitude.cpp
View File

@ -568,9 +568,9 @@ float Plane::rangefinder_correction(void)
*/ */
void Plane::rangefinder_height_update(void) void Plane::rangefinder_height_update(void)
{ {
float distance = rangefinder.distance_cm()*0.01f; float distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270)*0.01f;
if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) { if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
if (!rangefinder_state.have_initial_reading) { if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true; rangefinder_state.have_initial_reading = true;
rangefinder_state.initial_range = distance; rangefinder_state.initial_range = distance;
@ -586,10 +586,10 @@ void Plane::rangefinder_height_update(void)
// to misconfiguration or a faulty sensor // to misconfiguration or a faulty sensor
if (rangefinder_state.in_range_count < 10) { if (rangefinder_state.in_range_count < 10) {
if (!is_equal(distance, rangefinder_state.last_distance) && if (!is_equal(distance, rangefinder_state.last_distance) &&
fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) { fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01f) {
rangefinder_state.in_range_count++; rangefinder_state.in_range_count++;
} }
if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm()*0.01*0.2) { if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01*0.2) {
// changes by more than 20% of full range will reset counter // changes by more than 20% of full range will reset counter
rangefinder_state.in_range_count = 0; rangefinder_state.in_range_count = 0;
} }

4
ArduPlane/sensors.cpp
View File

@ -352,12 +352,12 @@ void Plane::update_sensor_status_flags(void)
} }
#endif #endif
if (rangefinder.num_sensors() > 0) { if (rangefinder.has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.rangefinder_landing) { if (g.rangefinder_landing) {
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;
} }
} }