Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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)
{
if (!rangefinder.has_data()) {
if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
// no sonar to report
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);
}
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()
{
uint16_t distance = 0;
if (rangefinder.status() == RangeFinder::RangeFinder_Good) {
distance = rangefinder.distance_cm();
if (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) {
distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_us : AP_HAL::micros64(),
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,
correction : rangefinder_state.correction
};

2
ArduPlane/Plane.h
View File

@ -200,7 +200,7 @@ private:
AP_InertialSensor ins;
RangeFinder rangefinder {serial_manager};
RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
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)
{
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) {
rangefinder_state.have_initial_reading = true;
rangefinder_state.initial_range = distance;
@ -586,10 +586,10 @@ void Plane::rangefinder_height_update(void)
// to misconfiguration or a faulty sensor
if (rangefinder_state.in_range_count < 10) {
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++;
}
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
rangefinder_state.in_range_count = 0;
}

4
ArduPlane/sensors.cpp
View File

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