From 6b8dfdfc10977022c264f3c8e9e0601430071342 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Sat, 17 Jun 2023 00:00:24 +0530 Subject: [PATCH] SITL: Add simulated nooploop tofsense --- libraries/SITL/SIM_RF_NoopLoop.cpp | 49 ++++++++++++++++++++++++++++++ libraries/SITL/SIM_RF_NoopLoop.h | 32 +++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 libraries/SITL/SIM_RF_NoopLoop.cpp create mode 100644 libraries/SITL/SIM_RF_NoopLoop.h diff --git a/libraries/SITL/SIM_RF_NoopLoop.cpp b/libraries/SITL/SIM_RF_NoopLoop.cpp new file mode 100644 index 0000000000..445b5862a2 --- /dev/null +++ b/libraries/SITL/SIM_RF_NoopLoop.cpp @@ -0,0 +1,49 @@ +/* + 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_NoopLoop.h" + +using namespace SITL; + +uint32_t RF_Nooploop::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + + int32_t alt_scaled = 2560*alt_cm; + buffer[0] = 0x57; + buffer[1] = 0x00; + buffer[2] = 0xff; + buffer[3] = 0x00; + buffer[4] = 0x9e; + buffer[5] = 0x8f; + buffer[6] = 0x00; + buffer[7] = 0x00; + buffer[8] = alt_scaled >> 8 & 0xff; + buffer[9] = alt_scaled >> 16 & 0xff; + buffer[10] = alt_scaled >> 24 & 0xff; + buffer[11] = 0x00; + buffer[12] = 0x03; + buffer[13] = 0x00; + buffer[14] = 0x06; + + // calculate checksum: + buffer[15] = 0; + for (uint8_t i=0; i<15; i++) { + buffer[15] += buffer[i]; + } + return 16; +} diff --git a/libraries/SITL/SIM_RF_NoopLoop.h b/libraries/SITL/SIM_RF_NoopLoop.h new file mode 100644 index 0000000000..1ee56d1528 --- /dev/null +++ b/libraries/SITL/SIM_RF_NoopLoop.h @@ -0,0 +1,32 @@ +/* + 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 +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_Nooploop : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + +}; + +}