SITL: add support for simulated serial rangefinders

SITL: add base class for serial rangefinder simulators

SITL: add Benewake rangefinder simulator

SITL: add support for simulated LightWareSerial rangefinder

SITL: add support for simulated Lanbao rangefinder

SITL: add support for simulated BLping rangefinder

SITL: add support for simulated LeddarOne rangefinder

SITL: add support for simulated uLanding rangefinders

SITL: add support for simulated MaxsonarSerialLV rangefinders

SITL: add support for simulated Wasp rangefinders

SITL: add support for simulated NMEA rangefinders

libraries/SITL/SIM_RF_BLping.cpp

@@ -0,0 +1,66 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the BLping rangefinder
+*/
+
+#include "SIM_RF_BLping.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
+
+
+using namespace SITL;
+
+uint32_t RF_BLping::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+#define BLPING_MSGID_ACK                    1
+#define BLPING_MSGID_NACK                   2
+#define BLPING_MSGID_SET_PING_INTERVAL      1004
+#define BLPING_MSGID_GET_DEVICE_ID          1201
+#define BLDPIN_MSGID_DISTANCE_SIMPLE        1211
+#define BLPING_MSGID_CONTINUOUS_START       1400
+
+    const uint32_t alt_mm = uint32_t(alt_cm * 10);
+    const uint8_t payload[] = {
+        uint8_t(alt_mm & 0xff),
+        uint8_t((alt_mm >> 8) & 0xff),
+        uint8_t((alt_mm >> 16) & 0xff),
+        uint8_t((alt_mm >> 24) & 0xff),
+    };
+    const uint16_t message_id = BLDPIN_MSGID_DISTANCE_SIMPLE;
+    const uint8_t src_device_id = 1;
+    const uint8_t dst_device_id = 0;
+    uint16_t offs = 0;
+    buffer[offs++] = 0x42;
+    buffer[offs++] = 0x52;
+    buffer[offs++] = ARRAY_SIZE(payload) & 0xff;
+    buffer[offs++] = ARRAY_SIZE(payload) >> 8;
+    buffer[offs++] = message_id & 0xff;
+    buffer[offs++] = message_id >> 8;
+    buffer[offs++] = src_device_id;
+    buffer[offs++] = dst_device_id;
+    memcpy(&buffer[offs], payload, ARRAY_SIZE(payload));
+    offs += ARRAY_SIZE(payload);
+    uint16_t crc = 0;
+    for (uint8_t i=0; i<offs; i++) {
+        crc += buffer[i];
+    }
+
+    buffer[offs++] = crc & 0xff;
+    buffer[offs++] = crc >> 8;
+
+    return offs;
+}

libraries/SITL/SIM_RF_BLping.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the BLping rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --uartF=sim:blping --speedup=1 -l 33.810313,-118.393867,0,185
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 23
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_BLping : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_RF_Benewake.cpp

@@ -0,0 +1,40 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for simulator for the Benewake Serial RangeFinders
+*/
+
+#include "SIM_RF_Benewake.h"
+
+using namespace SITL;
+
+uint32_t RF_Benewake::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    buffer[0] = 0x59;
+    buffer[1] = 0x59;
+    buffer[3] = alt_cm >> 8;
+    buffer[2] = alt_cm & 0xff;
+    buffer[4] = byte4();
+    buffer[5] = byte5();
+    buffer[6] = byte6();
+    buffer[7] = byte7();
+
+    // calculate checksum:
+    buffer[8] = 0;
+    for (uint8_t i=0; i<8; i++) {
+        buffer[8] += buffer[i];
+    }
+    return 9;
+}

libraries/SITL/SIM_RF_Benewake.h

@@ -0,0 +1,39 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for simulator for the Benewake Serial RangeFinders
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_Benewake : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+private:
+
+    virtual uint8_t byte4() const = 0;
+    virtual uint8_t byte5() const = 0;
+    virtual uint8_t byte6() const = 0;
+    virtual uint8_t byte7() const = 0;
+
+};
+
+}

libraries/SITL/SIM_RF_Benewake_TF02.h

@@ -0,0 +1,43 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Benewake TF02 RangeFinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf02 --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 19
+reboot
+
+*/
+
+#pragma once
+
+#include "SIM_RF_Benewake.h"
+
+namespace SITL {
+
+class RF_Benewake_TF02 : public RF_Benewake {
+public:
+
+    // see AP_RangeFinder_Benewake.cpp for definitions
+    uint8_t byte4() const override { return 1; } // strength low-bits
+    uint8_t byte5() const override { return 1; } // strength high-bits
+    uint8_t byte6() const override { return 7; } // reliability
+    uint8_t byte7() const override { return 0x06; } // exposure time
+
+};
+
+}

libraries/SITL/SIM_RF_Benewake_TF03.h

@@ -0,0 +1,43 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Benewake TF03 RangeFinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf03 --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 27
+reboot
+
+*/
+
+#pragma once
+
+#include "SIM_RF_Benewake.h"
+
+namespace SITL {
+
+class RF_Benewake_TF03 : public RF_Benewake {
+public:
+
+    // see AP_RangeFinder_Benewake.cpp for definitions
+    uint8_t byte4() const override { return 0; } // reserved
+    uint8_t byte5() const override { return 0; } // reserved
+    uint8_t byte6() const override { return 0; } // reserved
+    uint8_t byte7() const override { return 0; } // TF02 only
+
+};
+
+}

libraries/SITL/SIM_RF_Benewake_TFmini.h

@@ -0,0 +1,43 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the TFMini RangeFinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tfmini --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 20
+reboot
+
+*/
+
+#pragma once
+
+#include "SIM_RF_Benewake.h"
+
+namespace SITL {
+
+class RF_Benewake_TFmini : public RF_Benewake {
+public:
+
+    // see AP_RangeFinder_Benewake.cpp for definitions
+    uint8_t byte4() const override { return 1; } // strength L
+    uint8_t byte5() const override { return 1; } // strength H
+    uint8_t byte6() const override { return 0x07; } // distance mode (0x02=mm 0x07=cm)
+    uint8_t byte7() const override { return 0; } // TF02 only
+
+};
+
+}

libraries/SITL/SIM_RF_Lanbao.cpp

@@ -0,0 +1,39 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Lanbao rangefinder 
+*/
+
+#include "SIM_RF_Lanbao.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
+
+
+using namespace SITL;
+
+uint32_t RF_Lanbao::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    buffer[0] = 0xA5;
+    buffer[1] = 0x5A;
+    buffer[2] = (alt_cm * 10) >> 8;
+    buffer[3] = (alt_cm * 10) & 0xff;
+
+    const uint16_t crc = calc_crc_modbus(buffer, 4);
+    buffer[4] = crc & 0xff;
+    buffer[5] = crc >> 8;
+
+    return 6;
+}

libraries/SITL/SIM_RF_Lanbao.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Lanbao rangefinder 
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lanbao --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 26
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_Lanbao : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_RF_LeddarOne.cpp

@@ -0,0 +1,68 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the LeddarOne rangefinder
+*/
+
+#include "SIM_RF_LeddarOne.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
+
+using namespace SITL;
+
+uint32_t RF_LeddarOne::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    const uint8_t response_size = 25;
+    const uint16_t internal_temperature = 1245;
+    const uint16_t num_detections = 1;
+    const uint16_t first_distance = alt_cm * 10;
+    const uint16_t first_amplitude = 37;
+    const uint16_t second_distance = alt_cm * 10;
+    const uint16_t second_amplitude = 37;
+    const uint16_t third_distance = alt_cm * 10;
+    const uint16_t third_amplitude = 37;
+    const uint32_t now = AP_HAL::millis();
+
+    buffer[0] = 0x01;
+    buffer[1] = 0x04; // magic function number! LEDDARONE_MODOBUS_FUNCTION_CODE
+    buffer[2] = response_size;
+    buffer[3] = (now >> 16) & 0xff;
+    buffer[4] = (now >> 24) & 0xff;
+    buffer[5] = (now >> 0) & 0xff;
+    buffer[6] = (now >> 8) & 0xff;
+    buffer[7] = internal_temperature & 0xff;
+    buffer[8] = internal_temperature >> 8;
+    buffer[9] = num_detections >> 8;
+    buffer[10] = num_detections & 0xff;
+    buffer[11] = first_distance >> 8;
+    buffer[12] = first_distance & 0xff;
+    buffer[13] = first_amplitude >> 8;
+    buffer[14] = first_amplitude & 0xff;
+    buffer[15] = second_distance >> 8;
+    buffer[16] = second_distance & 0xff;
+    buffer[17] = second_amplitude >> 8;
+    buffer[18] = second_amplitude & 0xff;
+    buffer[19] = third_distance >> 8;
+    buffer[20] = third_distance & 0xff;
+    buffer[21] = third_amplitude >> 8;
+    buffer[22] = third_amplitude & 0xff;
+
+    const uint16_t crc = calc_crc_modbus(buffer, 23);
+    buffer[23] = crc & 0xff;
+    buffer[24] = crc >> 8;
+
+    return response_size;
+}

libraries/SITL/SIM_RF_LeddarOne.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the LeddarOne rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:leddarone --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 12
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_LeddarOne : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_RF_LightWareSerial.cpp

@@ -0,0 +1,54 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the serial LightWare rangefinder 
+*/
+
+#include "SIM_RF_LightWareSerial.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
+
+
+using namespace SITL;
+
+bool RF_LightWareSerial::check_synced()
+{
+    if (!synced) {
+        // just try to slurp a buffer in one hit:
+        char buffer[12] {};
+        ssize_t n = read_from_autopilot(buffer, ARRAY_SIZE(buffer) - 1);
+        if (n > 0) {
+            if (!strncmp(buffer, "www\r\n", ARRAY_SIZE(buffer))) {
+                gcs().send_text(MAV_SEVERITY_INFO, "Slurped a sync thing\n");
+                synced = true;
+            }
+        }
+    }
+    return synced;
+}
+
+void RF_LightWareSerial::update(float range)
+{
+    if (!check_synced()) {
+        return;
+    }
+    return SerialRangeFinder::update(range);
+}
+
+uint32_t RF_LightWareSerial::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    return snprintf((char*)buffer, buflen, "%f\r", alt_cm / 100.0f); // note tragic lack of snprintf return checking
+}

libraries/SITL/SIM_RF_LightWareSerial.h

@@ -0,0 +1,44 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the serial LightWare rangefinder 
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lightwareserial --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 8
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_LightWareSerial : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+    void update(float range) override;
+
+private:
+
+    bool check_synced();
+    bool synced;
+};
+
+}

libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp

@@ -0,0 +1,30 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Maxsonar Serial LV rangefinder
+*/
+
+#include "SIM_RF_MaxsonarSerialLV.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
+
+using namespace SITL;
+
+uint32_t RF_MaxsonarSerialLV::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    const float inches = alt_cm / 2.54f;
+    return snprintf((char*)buffer, buflen, "%u\r", (unsigned)inches);
+}

libraries/SITL/SIM_RF_MaxsonarSerialLV.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the MaxsonarSerialLV rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:maxsonarseriallv --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 13
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_MaxsonarSerialLV : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_RF_NMEA.cpp

@@ -0,0 +1,40 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the NMEA Serial rangefinder
+*/
+
+#include "SIM_RF_NMEA.h"
+
+#include <GCS_MAVLink/GCS.h>
+
+#include <stdio.h>
+#include <string.h>
+
+using namespace SITL;
+
+uint32_t RF_NMEA::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+// Format 2 DBT NMEA mode (e.g. $SMDBT,5.94,f,1.81,M,67)
+// Format 3 DPT NMEA mode (e.g. $SMDPT,1.81,0.066)
+
+    ssize_t ret = snprintf((char*)buffer, buflen, "$SMDPT,%f,%f", alt_cm/100.0f, 0.01f);
+    uint8_t checksum = 0;
+    for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped
+        checksum ^= buffer[i];
+    }
+    ret += snprintf((char*)&buffer[ret], buflen-ret, "*%02X\r\n", checksum);
+    return ret;
+}

libraries/SITL/SIM_RF_NMEA.h

@@ -0,0 +1,40 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the NMEA serial rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:nmea --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 17
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_NMEA : public SerialRangeFinder {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+private:
+
+};
+
+}

libraries/SITL/SIM_RF_Wasp.cpp

@@ -0,0 +1,117 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Wasp Serial rangefinder
+*/
+
+#include "SIM_RF_Wasp.h"
+
+#include <GCS_MAVLink/GCS.h>
+
+#include <stdio.h>
+#include <string.h>
+
+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<ARRAY_SIZE(string_configs); i++) {
+                if (!strncmp(&_buffer[1], string_configs[i].name, strlen(string_configs[i].name))) {
+                    uint8_t offs = strlen(string_configs[i].name);
+                    offs += 1; // for '>'
+                    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<ARRAY_SIZE(integer_configs); i++) {
+                if (!strncmp(&_buffer[1], integer_configs[i].name, strlen(integer_configs[i].name))) {
+                    uint8_t offs = strlen(integer_configs[i].name);
+                    offs += 1; // for '>'
+                    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);
+}

libraries/SITL/SIM_RF_Wasp.h

@@ -0,0 +1,80 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the Wasp serial rangefinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:wasp --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 18
+reboot
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_Wasp : public SerialRangeFinder {
+public:
+
+    void update(float range) override;
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+private:
+
+    void check_configuration();
+
+    struct {
+        bool go;
+        char format[16]; // e.g. ASCII
+        char baud[5]; // low or high
+        char lbe[7]; // big or little
+        int frq;
+        int aut;
+        int mavg;
+        int medf;
+        int avg;
+        int auv;
+    } config;
+
+    const struct {
+        const char *name;
+        char *value;
+    } string_configs[3] {
+        { "FMT", config.format },
+        { "BAUD", config.baud },
+        { "LBE", config.lbe },
+    };
+    const struct {
+        const char *name;
+        int *value;
+    } integer_configs[6] {
+        { "FRQ", &config.frq },
+        { "AUT", &config.aut },
+        { "MAVG", &config.mavg },
+        { "MEDF", &config.medf },
+        { "AVG", &config.avg },
+        { "AUV", &config.auv },
+    };
+
+    char _buffer[256]; // from-autopilot
+    uint8_t _buflen;
+
+};
+
+}

libraries/SITL/SIM_RF_uLanding.h

@@ -0,0 +1,29 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for simulator for the Benewake Serial RangeFinders
+*/
+
+#pragma once
+
+#include "SIM_SerialRangeFinder.h"
+
+namespace SITL {
+
+class RF_uLanding : public SerialRangeFinder {
+public:
+};
+
+}

libraries/SITL/SIM_RF_uLanding_v0.cpp

@@ -0,0 +1,36 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the uLanding v0 rangefinder
+*/
+
+#include "SIM_RF_uLanding_v0.h"
+
+using namespace SITL;
+
+uint32_t RF_uLanding_v0::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    const uint16_t reading = alt_cm / 2.5f;
+    buffer[0] = 0x48;
+    buffer[1] = reading & 0x7f;
+    buffer[2] = (reading >> 7) & 0xff;
+
+    // the detection routine is crap, frankly.  Needs lots of bytes
+    // *in one read* to work.
+    buffer[3] = 0x48;
+    buffer[4] = reading & 0x7f;
+    buffer[5] = (reading >> 7) & 0xff;
+    return 6;
+}

libraries/SITL/SIM_RF_uLanding_v0.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the uLanding v0 Serial RangeFinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ulanding_v0 --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 11
+reboot
+*/
+
+#pragma once
+
+#include "SIM_RF_uLanding.h"
+
+namespace SITL {
+
+class RF_uLanding_v0 : public RF_uLanding {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_RF_uLanding_v1.cpp

@@ -0,0 +1,34 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the uLanding v1 rangefinder
+*/
+
+#include "SIM_RF_uLanding_v1.h"
+
+using namespace SITL;
+
+uint32_t RF_uLanding_v1::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
+{
+    buffer[0] = 0xFE;
+    buffer[1] = 0; // unused?
+    buffer[2] = alt_cm & 0xff;
+    buffer[3] = alt_cm >> 8;
+    buffer[4] = 0; // unused?
+    // checksum:
+    buffer[5] = buffer[1] + buffer[2] + buffer[3] + buffer[4];
+
+    return 6;
+}

libraries/SITL/SIM_RF_uLanding_v1.h

@@ -0,0 +1,38 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the uLanding v1 Serial RangeFinder
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ulanding_v1 --speedup=1
+
+param set SERIAL5_PROTOCOL 9
+param set RNGFND1_TYPE 11
+reboot
+*/
+
+#pragma once
+
+#include "SIM_RF_uLanding.h"
+
+namespace SITL {
+
+class RF_uLanding_v1 : public RF_uLanding {
+public:
+
+    uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
+
+};
+
+}

libraries/SITL/SIM_SerialRangeFinder.cpp

@@ -0,0 +1,77 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for serial rangefinders
+*/
+
+#include "SIM_SerialRangeFinder.h"
+
+using namespace SITL;
+
+uint16_t SerialRangeFinder::calculate_range_cm(float range_value) const
+{
+    // swiped from sitl_rangefinder.cpp - we should unify them at some stage
+
+    const SITL *sitl = AP::sitl();
+
+    float altitude = range_value;
+    if (is_equal(range_value, -1.0f)) {  // Use SITL altitude as reading by default
+        altitude = AP::sitl()->height_agl;
+    }
+
+    // sensor position offset in body frame
+    const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
+
+    // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
+    if (!relPosSensorBF.is_zero()) {
+        // get a rotation matrix following DCM conventions (body to earth)
+        Matrix3f rotmat;
+        sitl->state.quaternion.rotation_matrix(rotmat);
+        // rotate the offset into earth frame
+        const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
+        // correct the altitude at the sensor
+        altitude -= relPosSensorEF.z;
+    }
+
+    // If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
+    // We adjust the reading with noise, glitch and scaler as the reading is on analog port.
+    if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) {
+        if (is_equal(range_value, -1.0f)) {  // disable for external reading that already handle this
+            // adjust for apparent altitude with roll
+            altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
+        }
+        // Add some noise on reading
+        altitude += sitl->sonar_noise * rand_float();
+    }
+
+    return altitude * 100.0f;
+}
+
+void SerialRangeFinder::update(float range)
+{
+    // just send a chunk of data at 1Hz:
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_sent_ms < 1000) {
+        return;
+    }
+    last_sent_ms = now;
+
+    uint8_t data[255];
+    const uint32_t packetlen = packet_for_alt(calculate_range_cm(range),
+                                              data,
+                                              ARRAY_SIZE(data));
+
+    write_to_autopilot((char*)data, packetlen);
+}

libraries/SITL/SIM_SerialRangeFinder.h

@@ -0,0 +1,47 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for serial rangefinders
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+#include <SITL/SITL.h>
+
+#include "SIM_SerialDevice.h"
+
+namespace SITL {
+
+class SerialRangeFinder : public SerialDevice {
+public:
+
+    SerialRangeFinder() {};
+
+    // update state
+    virtual void update(float range);
+
+    virtual uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) = 0;
+
+private:
+
+    uint32_t last_sent_ms;
+
+    uint16_t calculate_range_cm(float range_value) const;
+
+};
+
+}