Skip to content

Commit

Permalink
Check in MavLinkDroneController::update if we actually have a distanc…
Browse files Browse the repository at this point in the history
…e sensor
  • Loading branch information
cdyk committed Feb 13, 2018
1 parent 170cf69 commit 6a1657d
Showing 1 changed file with 14 additions and 10 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -787,16 +787,20 @@ struct MavLinkDroneController::impl {
mag_output.magnetic_field_body,
baro_output.pressure * 0.01f /*Pa to Milibar */, baro_output.altitude);

const auto& distance_output = getDistance()->getOutput();
float pitch, roll, yaw;
VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);

sendDistanceSensor(distance_output.min_distance / 100, //m -> cm
distance_output.max_distance / 100, //m -> cm
distance_output.distance,
0, //sensor type: //TODO: allow changing in settings?
77, //sensor id, //TODO: should this be something real?
pitch); //TODO: convert from radians to degrees?

const auto * distance = getDistance();
if (distance) {
const auto& distance_output = distance->getOutput();
float pitch, roll, yaw;
VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);

sendDistanceSensor(distance_output.min_distance / 100, //m -> cm
distance_output.max_distance / 100, //m -> cm
distance_output.distance,
0, //sensor type: //TODO: allow changing in settings?
77, //sensor id, //TODO: should this be something real?
pitch); //TODO: convert from radians to degrees?
}

const auto gps = getGps();
if (gps != nullptr) {
Expand Down

0 comments on commit 6a1657d

Please sign in to comment.