diff --git a/libraries/SITL/SIM_RF_LightWareSerialBinary.cpp b/libraries/SITL/SIM_RF_LightWareSerialBinary.cpp new file mode 100644 index 0000000000..549a66a11f --- /dev/null +++ b/libraries/SITL/SIM_RF_LightWareSerialBinary.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 serial LightWare rangefinder - binary mode +*/ + +#include "SIM_RF_LightWareSerialBinary.h" + +#include +#include + + +using namespace SITL; + +uint32_t RF_LightWareSerialBinary::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + // high byte is second 7 bits but high bit set + buffer[0] = ((alt_cm >> 7) & 0x7f) | (1<<7); + // low byte is just first 7 bits + buffer[1] = alt_cm & 0x7f; + return 2; +} diff --git a/libraries/SITL/SIM_RF_LightWareSerialBinary.h b/libraries/SITL/SIM_RF_LightWareSerialBinary.h new file mode 100644 index 0000000000..992a7b160a --- /dev/null +++ b/libraries/SITL/SIM_RF_LightWareSerialBinary.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 serial LightWare rangefinder - binary mode + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lightwareserial-binary --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 8 +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_LightWareSerialBinary : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + +}; + +}