Line data    Source code

       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             :   Simulator for the MAVLink Serial rangefinder
      17             : */
      18             : 
      19             : #include "SIM_RF_MAVLink.h"
      20             : 
      21             : #include <GCS_MAVLink/GCS.h>
      22             : 
      23             : #include <stdio.h>
      24             : #include <string.h>
      25             : 
      26             : using namespace SITL;
      27             : 
      28         392 : uint32_t RF_MAVLink::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen)
      29             : {
      30             :     mavlink_message_t msg;
      31         392 :     const uint8_t system_id = 32;
      32         392 :     const uint8_t component_id = 32;
      33         392 :     const uint16_t alt_cm = alt_m * 100;
      34         392 :     const mavlink_distance_sensor_t distance_sensor{
      35         392 :         .time_boot_ms = AP_HAL::millis(),
      36             :         .min_distance = 10, // cm
      37             :         .max_distance = 1000, // cm
      38             :         .current_distance = alt_cm,
      39             :         .type = 0,
      40             :         .id = 72, // ID
      41             :         .orientation = MAV_SENSOR_ROTATION_PITCH_270,
      42             :         .covariance = 255, // 255 is unknown covariance
      43             :         .horizontal_fov = 0, // 0 is unknown horizontal fov
      44             :         .vertical_fov = 0, // 0 is unknown vertical fov
      45             :         .quaternion = {0,0,0,0} // unknown/unused quat
      46         392 :     };
      47         392 :     const uint16_t len = mavlink_msg_distance_sensor_encode_status(system_id,
      48             :                                                                    component_id,
      49             :                                                                    &mav_status,
      50             :                                                                    &msg,
      51             :                                                                    &distance_sensor);
      52         392 :     if (len > buflen) {
      53           0 :         AP_HAL::panic("Insufficient buffer passed in");
      54             :     }
      55         392 :     const uint16_t retlen = mavlink_msg_to_send_buffer(buffer, &msg);
      56         392 :     return retlen;
      57             : }