From 2a2a103195882fd4fa86aafdfb1c3f3d79df3976 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 8 Aug 2023 14:34:39 +1000 Subject: [PATCH] SITL: add JAE JRE simulator --- libraries/SITL/SIM_RF_JRE.cpp | 50 +++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_RF_JRE.h | 46 ++++++++++++++++++++++++++++++++ 2 files changed, 96 insertions(+) create mode 100644 libraries/SITL/SIM_RF_JRE.cpp create mode 100644 libraries/SITL/SIM_RF_JRE.h diff --git a/libraries/SITL/SIM_RF_JRE.cpp b/libraries/SITL/SIM_RF_JRE.cpp new file mode 100644 index 0000000000..ddf44c67b7 --- /dev/null +++ b/libraries/SITL/SIM_RF_JRE.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 . + */ +/* + Base class for simulator for the NoopLoop TOFSense-F/P Serial RangeFinders +*/ + +#include "SIM_RF_JRE.h" + +using namespace SITL; + +uint32_t RF_JRE::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + uint16_t status = 0; + if (alt_cm > 50000) { + status |= 0x2; // NTRK, whatever that means... + } + + buffer[0] = 'R'; + buffer[1] = 'A'; + buffer[2] = 0x01; + buffer[3] = frame_count++; + buffer[4] = alt_cm >> 8; // altitide low + buffer[5] = alt_cm & 0xff; // Altitude-High, LSB + buffer[6] = 0x00; // reserved + buffer[7] = 0x00; // reserved + buffer[8] = 0x00; // reserved + buffer[9] = 0x00; // reserved + buffer[10] = 0x00; // FFT + buffer[11] = 0x00; // FFT + buffer[12] = status >> 8; // FFT + buffer[13] = status & 0xff; // FFT + + const uint16_t crc = crc16_ccitt_r(&buffer[0], 14, 0xffff, 0xffff); + buffer[14] = crc & 0xff; + buffer[15] = crc >> 8; + + return 16; +} diff --git a/libraries/SITL/SIM_RF_JRE.h b/libraries/SITL/SIM_RF_JRE.h new file mode 100644 index 0000000000..d10c77a525 --- /dev/null +++ b/libraries/SITL/SIM_RF_JRE.h @@ -0,0 +1,46 @@ +/* + 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 JAE JRE radio altimiter + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:jre --speedup=1 -L KalaupapaCliffs --map + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 38 +reboot + +graph RANGEFINDER.distance + +arm throttle +rc 3 1600 + +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_JRE : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + +private: + uint8_t frame_count; // sequence number +}; + +}