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 Wasp Serial rangefinder
      17             : */
      18             : 
      19             : #include "SIM_RF_Wasp.h"
      20             : 
      21             : #include <GCS_MAVLink/GCS.h>
      22             : 
      23             : #include <stdio.h>
      24             : #include <string.h>
      25             : 
      26             : using namespace SITL;
      27             : 
      28       85099 : void RF_Wasp::check_configuration()
      29             : {
      30       85099 :     const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
      31       85099 :     if (n <= 0) {
      32       85091 :         return;
      33             :     }
      34           8 :     _buflen += n;
      35             : 
      36             :     // ensure we have an entire line:
      37           8 :     const char *cr = strchr(_buffer, '\n');
      38           8 :     if (cr == nullptr) {
      39           0 :         if (_buflen == ARRAY_SIZE(_buffer) - 1) {
      40             :             // nuke it all
      41           0 :             memset(_buffer, '\0', ARRAY_SIZE(_buffer));
      42           0 :             _buflen = 0;
      43             :         }
      44           0 :         return;
      45             :     }
      46           8 :     if (!strncmp(_buffer, ">GO\n", _buflen)) {
      47           1 :         config.go = true;
      48           1 :         const char *response = "GO\n";
      49           1 :         write_to_autopilot(response, strlen(response));
      50           7 :     } else if (_buffer[0] == '>') {
      51           7 :         bool set = false;
      52           7 :         if (!set) {
      53             :             // check for string settings
      54          25 :             for (uint8_t i=0; i<ARRAY_SIZE(string_configs); i++) {
      55          19 :                 if (!strncmp(&_buffer[1], string_configs[i].name, strlen(string_configs[i].name))) {
      56           1 :                     uint8_t offs = strlen(string_configs[i].name);
      57           1 :                     offs += 1; // for '>'
      58           1 :                     offs += 1; // for space
      59           1 :                     strncpy(string_configs[i].value, &_buffer[offs], MIN(ARRAY_SIZE(config.format), unsigned(cr - _buffer - offs - 1))); // -1 for the lf, -1 for the cr
      60             : //                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: config (%s) (%s)", string_configs[i].name, string_configs[i].value);
      61             :                     char response[128];
      62           1 :                     const size_t x = snprintf(response,
      63             :                                               ARRAY_SIZE(response),
      64             :                                               "%s %s\n",
      65           1 :                                               string_configs[i].name,
      66           1 :                                               string_configs[i].value);
      67           1 :                     write_to_autopilot(response, x);
      68           1 :                     set = true;
      69           1 :                     break;
      70             :                 }
      71             :             }
      72             :         }
      73           7 :         if (!set) {
      74             :             // check for integer settings
      75          21 :             for (uint8_t i=0; i<ARRAY_SIZE(integer_configs); i++) {
      76          21 :                 if (!strncmp(&_buffer[1], integer_configs[i].name, strlen(integer_configs[i].name))) {
      77           6 :                     uint8_t offs = strlen(integer_configs[i].name);
      78           6 :                     offs += 1; // for '>'
      79           6 :                     offs += 1; // for space
      80           6 :                     char tmp[32]{};
      81           6 :                     strncpy(tmp, &_buffer[offs], MIN(ARRAY_SIZE(config.format), unsigned(cr - _buffer - offs - 1))); // -1 for the lf, -1 for the cr
      82           6 :                     *(integer_configs[i].value) = atoi(tmp);
      83             : //                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: config (%s) (%d)", integer_configs[i].name, *(integer_configs[i].value));
      84             :                     char response[128];
      85           6 :                     const size_t x = snprintf(response,
      86             :                                               ARRAY_SIZE(response),
      87             :                                               "%s %d\n",
      88           6 :                                               integer_configs[i].name,
      89           6 :                                               *(integer_configs[i].value));
      90           6 :                     write_to_autopilot(response, x);
      91           6 :                     set = true;
      92           6 :                     break;
      93             :                 }
      94             :             }
      95             :         }
      96           7 :         if (!set) {
      97           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: unknown setting (%s)", &_buffer[0]);
      98             :         }
      99             :     }
     100             : 
     101             :     // just nuke everything in the buffer, not just what we just
     102             :     // processed.  This is until we sort out the extra-cr thing
     103           8 :     memset(_buffer, '\0', ARRAY_SIZE(_buffer));
     104           8 :     _buflen = 0;
     105             : }
     106             : 
     107       85099 : void RF_Wasp::update(float range)
     108             : {
     109       85099 :     check_configuration();
     110       85099 :     return SerialRangeFinder::update(range);
     111             : }
     112             : 
     113             : 
     114        1417 : uint32_t RF_Wasp::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen)
     115             : {
     116             :     // the Wasp driver does not consider 0 a valid reading, so you end
     117             :     // up getting 0 samples back if you send exactly zero all the
     118             :     // time.  So munge it a little bit:
     119        1417 :     if (alt_m < 0.01) {
     120        1030 :         alt_m = 0.01;
     121             :     }
     122        1417 :     return snprintf((char*)buffer, buflen, "%f\n", alt_m);
     123             : }