diff --git a/libraries/SITL/SIM_RF_Ainstein_LR_D1.cpp b/libraries/SITL/SIM_RF_Ainstein_LR_D1.cpp new file mode 100644 index 0000000000..21cf19819e --- /dev/null +++ b/libraries/SITL/SIM_RF_Ainstein_LR_D1.cpp @@ -0,0 +1,50 @@ +/* + 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 Ainstein LR-D1 Serial rangefinder +*/ + +#include "SIM_RF_Ainstein_LR_D1.h" + +using namespace SITL; + +uint32_t RF_Ainstein_LR_D1::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + const uint8_t PACKET_LEN = 32; + + if (buflen < PACKET_LEN) { + return 0; + } + + uint8_t malfunction_alert = 0; + + const uint8_t snr = (alt_cm == 0xFFFF) ? 0 : 100; + + buffer[0] = 0xEB; // packet header msb + buffer[1] = 0x90; // packet header lsb + buffer[2] = 0; // device_id + buffer[3] = 28; // length + buffer[4] = malfunction_alert; + buffer[5] = 1; // object count + buffer[6] = HIGHBYTE(alt_cm); + buffer[7] = LOWBYTE(alt_cm); + buffer[8] = snr; + buffer[9] = 0; // speed high + buffer[10] = 0; // speed low + memset(&buffer[11], 0xff, 20); // unused + buffer[31] = crc_sum_of_bytes(&buffer[3], PACKET_LEN-4); // minus headerMSB, headerLSB, device_id and checksum + + return PACKET_LEN; +} diff --git a/libraries/SITL/SIM_RF_Ainstein_LR_D1.h b/libraries/SITL/SIM_RF_Ainstein_LR_D1.h new file mode 100644 index 0000000000..849979f3dc --- /dev/null +++ b/libraries/SITL/SIM_RF_Ainstein_LR_D1.h @@ -0,0 +1,55 @@ +/* + 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 Ainstein LR-D1 serial rangefinder + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:ainsteinlrd1 --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 42 +graph RANGEFINDER.distance +graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance +reboot + +arm throttle +rc 3 1600 + +# to pass canned data through the simulator: +/Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=file:libraries/AP_RangeFinder/examples/captures/ainstein_lr-d1.bin --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 42 +reboot + +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_Ainstein_LR_D1 : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + + uint16_t reading_interval_ms() const override { return 100; } + +private: + +}; + +}