/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* Simulator for the Wasp Serial rangefinder */ #include "SIM_RF_Wasp.h" #include #include #include using namespace SITL; void RF_Wasp::check_configuration() { const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1); if (n <= 0) { return; } _buflen += n; // ensure we have an entire line: const char *cr = strchr(_buffer, '\n'); if (cr == nullptr) { if (_buflen == ARRAY_SIZE(_buffer) - 1) { // nuke it all memset(_buffer, '\0', ARRAY_SIZE(_buffer)); _buflen = 0; } return; } if (!strncmp(_buffer, ">GO\n", _buflen)) { config.go = true; const char *response = "GO\n"; write_to_autopilot(response, strlen(response)); } else if (_buffer[0] == '>') { bool set = false; if (!set) { // check for string settings for (uint8_t i=0; i' offs += 1; // for space 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 // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: config (%s) (%s)", string_configs[i].name, string_configs[i].value); char response[128]; const size_t x = snprintf(response, ARRAY_SIZE(response), "%s %s\n", string_configs[i].name, string_configs[i].value); write_to_autopilot(response, x); set = true; break; } } } if (!set) { // check for integer settings for (uint8_t i=0; i' offs += 1; // for space char tmp[32]{}; strncpy(tmp, &_buffer[offs], MIN(ARRAY_SIZE(config.format), unsigned(cr - _buffer - offs - 1))); // -1 for the lf, -1 for the cr *(integer_configs[i].value) = atoi(tmp); // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: config (%s) (%d)", integer_configs[i].name, *(integer_configs[i].value)); char response[128]; const size_t x = snprintf(response, ARRAY_SIZE(response), "%s %d\n", integer_configs[i].name, *(integer_configs[i].value)); write_to_autopilot(response, x); set = true; break; } } } if (!set) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Wasp: unknown setting (%s)", &_buffer[0]); } } // just nuke everything in the buffer, not just what we just // processed. This is until we sort out the extra-cr thing memset(_buffer, '\0', ARRAY_SIZE(_buffer)); _buflen = 0; } void RF_Wasp::update(float range) { check_configuration(); return SerialRangeFinder::update(range); } uint32_t RF_Wasp::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) { return snprintf((char*)buffer, buflen, "%f\n", alt_cm/100.0f); }