APM:Libraries
AP_RangeFinder_MAVLink.cpp
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 
16 #include "AP_RangeFinder_MAVLink.h"
17 #include <AP_HAL/AP_HAL.h>
18 
19 
20 
21 extern const AP_HAL::HAL& hal;
22 
23 /*
24  The constructor also initialises the rangefinder. Note that this
25  constructor is not called until detect() returns true, so we
26  already know that we should setup the rangefinder
27 */
30 {
32  distance_cm = 0;
33 }
34 
35 /*
36  detect if a MAVLink rangefinder is connected. We'll detect by
37  checking a parameter.
38 */
40 {
41  // Assume that if the user set the RANGEFINDER_TYPE parameter to MAVLink,
42  // there is an attached MAVLink rangefinder
43  return true;
44 }
45 
46 /*
47  Set the distance based on a MAVLINK message
48 */
49 void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
50 {
51  mavlink_distance_sensor_t packet;
52  mavlink_msg_distance_sensor_decode(msg, &packet);
53 
54  // only accept distances for downward facing sensors
55  if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) {
57  distance_cm = packet.current_distance;
58  }
59  sensor_type = (MAV_DISTANCE_SENSOR)packet.type;
60 }
61 
62 /*
63  update the state of the sensor
64 */
66 {
67  //Time out on incoming data; if we don't get new
68  //data in 500ms, dump it
71  state.distance_cm = 0;
72  } else {
74  update_status();
75  }
76 }
uint16_t distance_cm() const
uint32_t millis()
Definition: system.cpp:157
void set_status(RangeFinder::RangeFinder_Status status)
RangeFinder::RangeFinder_State & state