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 LD06 proximity sensors
      17             : */
      18             : 
      19             : #include "SIM_config.h"
      20             : 
      21             : #if AP_SIM_PS_LD06_ENABLED
      22             : 
      23             : #include "SIM_PS_LD06.h"
      24             : 
      25             : #include <GCS_MAVLink/GCS.h>
      26             : #include <AP_BoardConfig/AP_BoardConfig.h>
      27             : #include <stdio.h>
      28             : #include <errno.h>
      29             : #include <AP_HAL/utility/sparse-endian.h>
      30             : 
      31             : using namespace SITL;
      32             : 
      33           0 : uint32_t PS_LD06::packet_for_location(const Location &location,
      34             :                                       uint8_t *data,
      35             :                                       uint8_t buflen)
      36             : {
      37           0 :     return 0;
      38             : }
      39             : 
      40       52407 : void PS_LD06::update_input()
      41             : {
      42             :     // just discard any input
      43             :     char buffer[256];
      44       52407 :     uint8_t buflen = 0;
      45       52407 :     read_from_autopilot(&buffer[buflen], ARRAY_SIZE(buffer) - buflen - 1);
      46       52407 : }
      47             : 
      48       52407 : void PS_LD06::update_output(const Location &location)
      49             : {
      50       52407 :     const uint32_t now_ms = AP_HAL::millis();
      51       52407 :     if (last_scan_output_time_ms == 0) {
      52           3 :         last_scan_output_time_ms = now_ms;
      53           3 :         return;
      54             :     }
      55       52404 :     const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms);
      56             : 
      57             :     // the driver and device only send/handle 12 readings at a time at
      58             :     // the moment
      59       52404 :     const uint32_t sample_count = 12;
      60             : 
      61       52404 :     const uint32_t samples_per_second = 4500;
      62       52404 :     const float samples_per_ms = samples_per_second / 1000.0f;
      63             : 
      64       52404 :     const uint32_t required_time_delta_ms = sample_count * samples_per_ms;
      65             : 
      66       52404 :     if (time_delta_ms < required_time_delta_ms) {
      67       30604 :         return;
      68             :     }
      69             :     // sanity check we're being called often enough:
      70       21800 :     if (time_delta_ms / samples_per_ms > sample_count) {
      71           0 :         AP_HAL::panic("Not being called often enough");
      72             :     }
      73             : 
      74       21800 :     const float degrees_per_s = 2152;  // see example datasheet page 8
      75       21800 :     const float degrees_per_ms = degrees_per_s / 1000.0f;
      76       21800 :     const float degrees_per_sample = degrees_per_ms / samples_per_ms;
      77             : 
      78             :     // ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta_ms, degrees_per_sample);
      79             : 
      80       21800 :     last_scan_output_time_ms += sample_count/samples_per_ms;
      81             : 
      82       21800 :     const uint32_t required_send_buffer_size = 11 + 3*sample_count;
      83       21800 :     if (required_send_buffer_size > send_buffer_size) {
      84           1 :         if (send_buffer != nullptr) {
      85           0 :             free(send_buffer);
      86             :         }
      87           1 :         send_buffer = (uint8_t*)malloc(required_send_buffer_size);
      88           1 :         if (send_buffer == nullptr) {
      89           0 :             AP_BoardConfig::allocation_error("LD06 send buffer");
      90             :         }
      91           1 :         send_buffer_size = required_send_buffer_size;
      92             :     }
      93             : 
      94             :     struct PACKED PacketStart {
      95             :         uint8_t preamble;
      96             :         uint8_t length : 5;
      97             :         uint8_t reserved : 3;
      98             :         uint16_t angular_rate;  // degrees/second
      99             :         uint16_t start_angle;  // centidegrees
     100             :     };
     101             :     struct PACKED Measurement {
     102             :         uint16_t distance;
     103             :         uint8_t confidence;
     104             :     };
     105             :     struct PACKED PacketEnd {
     106             :         uint16_t end_angle;
     107             :         uint16_t timestamp;
     108             :         uint8_t crc;
     109             :     };
     110             : 
     111       21800 :     PacketStart &packet_start = *((PacketStart*)send_buffer);
     112             : 
     113       21800 :     packet_start.preamble = 0x54;
     114       21800 :     packet_start.length = sample_count;
     115       21800 :     packet_start.angular_rate = htole16(degrees_per_s);
     116       21800 :     packet_start.start_angle = htole16(last_degrees_bf * 100);
     117             : 
     118       21800 :     uint16_t offset = sizeof(PacketStart);
     119             : 
     120      283400 :     for (uint32_t i=0; i<sample_count; i++) {
     121      261600 :         const float current_degrees_bf = fmod((last_degrees_bf + degrees_per_sample), 360.0f);
     122      261600 :         last_degrees_bf = current_degrees_bf;
     123             : 
     124      261600 :         float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
     125             :         // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
     126             : 
     127      261600 :         uint8_t confidence = 29; // random number; driver checks this
     128      261600 :         if (distance > 12.0) {  // 12 metre max range
     129             :             // return 0 for out-of-range; is this correct?
     130      109672 :             distance = 0.0f;
     131      109672 :             confidence = 0;
     132             :         }
     133             : 
     134      261600 :         Measurement &measurement = *((Measurement*)&send_buffer[offset]);
     135             : 
     136      261600 :         measurement.distance = htole16(distance * 1000);  // m -> mm
     137      261600 :         measurement.confidence = confidence;
     138             : 
     139      261600 :         offset += sizeof(Measurement);
     140             :     }
     141             : 
     142       21800 :     PacketEnd &packet_end = *((PacketEnd*)&send_buffer[offset]);
     143             : 
     144       21800 :     packet_end.end_angle = htole16(last_degrees_bf * 100);  // centidegrees
     145       21800 :     packet_end.timestamp = htole16(0);  // milliseconds
     146       21800 :     offset += sizeof(PacketEnd);
     147             : 
     148       21800 :     packet_end.crc = crc8_generic(&send_buffer[0], offset-1, 0x4D);
     149             : 
     150       21800 :     const ssize_t ret = write_to_autopilot((const char*)send_buffer, offset);
     151       21800 :     if (ret != offset) {
     152             :         // abort();  // there are startup issues with this, we fill things up before the driver is ready
     153             :     }
     154             : }
     155             : 
     156       52407 : void PS_LD06::update(const Location &location)
     157             : {
     158       52407 :     update_input();
     159       52407 :     update_output(location);
     160       52407 : }
     161             : 
     162             : #endif  // AP_SIM_PS_LD06_ENABLED