From df0233a7d1e50d0ad949eedc394405725d90e874 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 7 Nov 2019 15:03:44 +1100 Subject: [PATCH] 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 | 66 ++++++++++++ libraries/SITL/SIM_RF_BLping.h | 38 +++++++ libraries/SITL/SIM_RF_Benewake.cpp | 40 +++++++ libraries/SITL/SIM_RF_Benewake.h | 39 +++++++ libraries/SITL/SIM_RF_Benewake_TF02.h | 43 ++++++++ libraries/SITL/SIM_RF_Benewake_TF03.h | 43 ++++++++ libraries/SITL/SIM_RF_Benewake_TFmini.h | 43 ++++++++ libraries/SITL/SIM_RF_Lanbao.cpp | 39 +++++++ libraries/SITL/SIM_RF_Lanbao.h | 38 +++++++ libraries/SITL/SIM_RF_LeddarOne.cpp | 68 ++++++++++++ libraries/SITL/SIM_RF_LeddarOne.h | 38 +++++++ libraries/SITL/SIM_RF_LightWareSerial.cpp | 54 ++++++++++ libraries/SITL/SIM_RF_LightWareSerial.h | 44 ++++++++ libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp | 30 ++++++ libraries/SITL/SIM_RF_MaxsonarSerialLV.h | 38 +++++++ libraries/SITL/SIM_RF_NMEA.cpp | 40 +++++++ libraries/SITL/SIM_RF_NMEA.h | 40 +++++++ libraries/SITL/SIM_RF_Wasp.cpp | 117 +++++++++++++++++++++ libraries/SITL/SIM_RF_Wasp.h | 80 ++++++++++++++ libraries/SITL/SIM_RF_uLanding.cpp | 0 libraries/SITL/SIM_RF_uLanding.h | 29 +++++ libraries/SITL/SIM_RF_uLanding_v0.cpp | 36 +++++++ libraries/SITL/SIM_RF_uLanding_v0.h | 38 +++++++ libraries/SITL/SIM_RF_uLanding_v1.cpp | 34 ++++++ libraries/SITL/SIM_RF_uLanding_v1.h | 38 +++++++ libraries/SITL/SIM_SerialRangeFinder.cpp | 77 ++++++++++++++ libraries/SITL/SIM_SerialRangeFinder.h | 47 +++++++++ 27 files changed, 1237 insertions(+) create mode 100644 libraries/SITL/SIM_RF_BLping.cpp create mode 100644 libraries/SITL/SIM_RF_BLping.h create mode 100644 libraries/SITL/SIM_RF_Benewake.cpp create mode 100644 libraries/SITL/SIM_RF_Benewake.h create mode 100644 libraries/SITL/SIM_RF_Benewake_TF02.h create mode 100644 libraries/SITL/SIM_RF_Benewake_TF03.h create mode 100644 libraries/SITL/SIM_RF_Benewake_TFmini.h create mode 100644 libraries/SITL/SIM_RF_Lanbao.cpp create mode 100644 libraries/SITL/SIM_RF_Lanbao.h create mode 100644 libraries/SITL/SIM_RF_LeddarOne.cpp create mode 100644 libraries/SITL/SIM_RF_LeddarOne.h create mode 100644 libraries/SITL/SIM_RF_LightWareSerial.cpp create mode 100644 libraries/SITL/SIM_RF_LightWareSerial.h create mode 100644 libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp create mode 100644 libraries/SITL/SIM_RF_MaxsonarSerialLV.h create mode 100644 libraries/SITL/SIM_RF_NMEA.cpp create mode 100644 libraries/SITL/SIM_RF_NMEA.h create mode 100644 libraries/SITL/SIM_RF_Wasp.cpp create mode 100644 libraries/SITL/SIM_RF_Wasp.h create mode 100644 libraries/SITL/SIM_RF_uLanding.cpp create mode 100644 libraries/SITL/SIM_RF_uLanding.h create mode 100644 libraries/SITL/SIM_RF_uLanding_v0.cpp create mode 100644 libraries/SITL/SIM_RF_uLanding_v0.h create mode 100644 libraries/SITL/SIM_RF_uLanding_v1.cpp create mode 100644 libraries/SITL/SIM_RF_uLanding_v1.h create mode 100644 libraries/SITL/SIM_SerialRangeFinder.cpp create mode 100644 libraries/SITL/SIM_SerialRangeFinder.h diff --git a/libraries/SITL/SIM_RF_BLping.cpp b/libraries/SITL/SIM_RF_BLping.cpp new file mode 100644 index 0000000000..1801b30feb --- /dev/null +++ b/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 . + */ +/* + Simulator for the BLping rangefinder +*/ + +#include "SIM_RF_BLping.h" + +#include +#include + + +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> 8; + + return offs; +} diff --git a/libraries/SITL/SIM_RF_BLping.h b/libraries/SITL/SIM_RF_BLping.h new file mode 100644 index 0000000000..e108862c94 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_Benewake.cpp b/libraries/SITL/SIM_RF_Benewake.cpp new file mode 100644 index 0000000000..e5e95e46a7 --- /dev/null +++ b/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 . + */ +/* + 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; +} diff --git a/libraries/SITL/SIM_RF_Benewake.h b/libraries/SITL/SIM_RF_Benewake.h new file mode 100644 index 0000000000..341a846ba2 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_Benewake_TF02.h b/libraries/SITL/SIM_RF_Benewake_TF02.h new file mode 100644 index 0000000000..7c6b271d64 --- /dev/null +++ b/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 . + */ +/* + 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 + +}; + +} diff --git a/libraries/SITL/SIM_RF_Benewake_TF03.h b/libraries/SITL/SIM_RF_Benewake_TF03.h new file mode 100644 index 0000000000..f4c2bc9051 --- /dev/null +++ b/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 . + */ +/* + 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 + +}; + +} diff --git a/libraries/SITL/SIM_RF_Benewake_TFmini.h b/libraries/SITL/SIM_RF_Benewake_TFmini.h new file mode 100644 index 0000000000..f69f3ffd13 --- /dev/null +++ b/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 . + */ +/* + 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 + +}; + +} diff --git a/libraries/SITL/SIM_RF_Lanbao.cpp b/libraries/SITL/SIM_RF_Lanbao.cpp new file mode 100644 index 0000000000..5ea0a3a698 --- /dev/null +++ b/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 . + */ +/* + Simulator for the Lanbao rangefinder +*/ + +#include "SIM_RF_Lanbao.h" + +#include +#include + + +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; +} diff --git a/libraries/SITL/SIM_RF_Lanbao.h b/libraries/SITL/SIM_RF_Lanbao.h new file mode 100644 index 0000000000..e5fb704004 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_LeddarOne.cpp b/libraries/SITL/SIM_RF_LeddarOne.cpp new file mode 100644 index 0000000000..078c4bc19d --- /dev/null +++ b/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 . + */ +/* + Simulator for the LeddarOne rangefinder +*/ + +#include "SIM_RF_LeddarOne.h" + +#include +#include + +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; +} diff --git a/libraries/SITL/SIM_RF_LeddarOne.h b/libraries/SITL/SIM_RF_LeddarOne.h new file mode 100644 index 0000000000..6ba01c5506 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_LightWareSerial.cpp b/libraries/SITL/SIM_RF_LightWareSerial.cpp new file mode 100644 index 0000000000..5015cf5cd7 --- /dev/null +++ b/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 . + */ +/* + Simulator for the serial LightWare rangefinder +*/ + +#include "SIM_RF_LightWareSerial.h" + +#include +#include + + +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 +} diff --git a/libraries/SITL/SIM_RF_LightWareSerial.h b/libraries/SITL/SIM_RF_LightWareSerial.h new file mode 100644 index 0000000000..e0d7c44b8e --- /dev/null +++ b/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 . + */ +/* + 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; +}; + +} diff --git a/libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp b/libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp new file mode 100644 index 0000000000..95be6a8f24 --- /dev/null +++ b/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 . + */ +/* + Simulator for the Maxsonar Serial LV rangefinder +*/ + +#include "SIM_RF_MaxsonarSerialLV.h" + +#include +#include + +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); +} diff --git a/libraries/SITL/SIM_RF_MaxsonarSerialLV.h b/libraries/SITL/SIM_RF_MaxsonarSerialLV.h new file mode 100644 index 0000000000..54d739d709 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_NMEA.cpp b/libraries/SITL/SIM_RF_NMEA.cpp new file mode 100644 index 0000000000..5e1bae4104 --- /dev/null +++ b/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 . + */ +/* + Simulator for the NMEA Serial rangefinder +*/ + +#include "SIM_RF_NMEA.h" + +#include + +#include +#include + +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. + */ +/* + 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: + +}; + +} diff --git a/libraries/SITL/SIM_RF_Wasp.cpp b/libraries/SITL/SIM_RF_Wasp.cpp new file mode 100644 index 0000000000..94eabc9b02 --- /dev/null +++ b/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 . + */ +/* + 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); +} diff --git a/libraries/SITL/SIM_RF_Wasp.h b/libraries/SITL/SIM_RF_Wasp.h new file mode 100644 index 0000000000..01a9cb8f4a --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_uLanding.cpp b/libraries/SITL/SIM_RF_uLanding.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/SITL/SIM_RF_uLanding.h b/libraries/SITL/SIM_RF_uLanding.h new file mode 100644 index 0000000000..b26feb6fe6 --- /dev/null +++ b/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 . + */ +/* + Base class for simulator for the Benewake Serial RangeFinders +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_uLanding : public SerialRangeFinder { +public: +}; + +} diff --git a/libraries/SITL/SIM_RF_uLanding_v0.cpp b/libraries/SITL/SIM_RF_uLanding_v0.cpp new file mode 100644 index 0000000000..2234968eb6 --- /dev/null +++ b/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 . + */ +/* + 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; +} diff --git a/libraries/SITL/SIM_RF_uLanding_v0.h b/libraries/SITL/SIM_RF_uLanding_v0.h new file mode 100644 index 0000000000..0c88e73560 --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_RF_uLanding_v1.cpp b/libraries/SITL/SIM_RF_uLanding_v1.cpp new file mode 100644 index 0000000000..0013c35ae7 --- /dev/null +++ b/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 . + */ +/* + 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; +} diff --git a/libraries/SITL/SIM_RF_uLanding_v1.h b/libraries/SITL/SIM_RF_uLanding_v1.h new file mode 100644 index 0000000000..9b810a012c --- /dev/null +++ b/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 . + */ +/* + 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; + +}; + +} diff --git a/libraries/SITL/SIM_SerialRangeFinder.cpp b/libraries/SITL/SIM_SerialRangeFinder.cpp new file mode 100644 index 0000000000..4655614af9 --- /dev/null +++ b/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 . + */ +/* + 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); +} diff --git a/libraries/SITL/SIM_SerialRangeFinder.h b/libraries/SITL/SIM_SerialRangeFinder.h new file mode 100644 index 0000000000..b843408332 --- /dev/null +++ b/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 . + */ +/* + Base class for serial rangefinders +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +#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; + +}; + +}