diff --git a/libraries/SITL/SIM_RF_GYUS42v2.cpp b/libraries/SITL/SIM_RF_GYUS42v2.cpp new file mode 100644 index 0000000000..0ab5c40ce9 --- /dev/null +++ b/libraries/SITL/SIM_RF_GYUS42v2.cpp @@ -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 GYUS42v2 Serial rangefinder +*/ + +#include "SIM_RF_GYUS42v2.h" + +#include +#include + +using namespace SITL; + +uint32_t RF_GYUS42v2::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + if (buflen < 7) { + return 0; + } + buffer[0] = 0x5A; + buffer[1] = 0; // FIXME + buffer[2] = 0; // FIXME + buffer[3] = 0; // FIXME + buffer[4] = alt_cm >> 8; + buffer[5] = alt_cm & 0xFF; + buffer[6] = 0; + for (uint8_t i=0; i<6; i++) { + buffer[6] += buffer[i]; + } + + return 7; +} diff --git a/libraries/SITL/SIM_RF_GYUS42v2.h b/libraries/SITL/SIM_RF_GYUS42v2.h new file mode 100644 index 0000000000..308123dd18 --- /dev/null +++ b/libraries/SITL/SIM_RF_GYUS42v2.h @@ -0,0 +1,48 @@ +/* + 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 GY-US42-v2 serial rangefinder + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:gyus42v2 --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 31 +graph RANGEFINDER.distance +graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance +reboot + +arm throttle +rc 3 1600 +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_GYUS42v2 : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + + // TODO: work this out + uint16_t reading_interval_ms() const override { return 100; } + +private: + +}; + +}