diff --git a/libraries/SITL/SIM_Frsky.cpp b/libraries/SITL/SIM_Frsky.cpp new file mode 100644 index 0000000000..720f9582c7 --- /dev/null +++ b/libraries/SITL/SIM_Frsky.cpp @@ -0,0 +1,47 @@ +/* + 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 FrSky telemetery +*/ + +#include "SIM_Frsky.h" + +using namespace SITL; + +const char *Frsky::dataid_string(DataID id) +{ + switch (id) { + case DataID::GPS_ALT_BP: return "GPS_ALT_BP"; + case DataID::TEMP1: return "TEMP1"; + case DataID::FUEL: return "FUEL"; + case DataID::TEMP2: return "TEMP2"; + case DataID::GPS_ALT_AP: return "GPS_ALT_AP"; + case DataID::BARO_ALT_BP: return "BARO_ALT_BP"; + case DataID::GPS_SPEED_BP: return "GPS_SPEED_BP"; + case DataID::GPS_LONG_BP: return "GPS_LONG_BP"; + case DataID::GPS_LAT_BP: return "GPS_LAT_BP"; + case DataID::GPS_COURS_BP: return "GPS_COURS_BP"; + case DataID::GPS_SPEED_AP: return "GPS_SPEED_AP"; + case DataID::GPS_LONG_AP: return "GPS_LONG_AP"; + case DataID::GPS_LAT_AP: return "GPS_LAT_AP"; + case DataID::BARO_ALT_AP: return "BARO_ALT_AP"; + case DataID::GPS_LONG_EW: return "GPS_LONG_EW"; + case DataID::GPS_LAT_NS: return "GPS_LAT_NS"; + case DataID::CURRENT: return "CURRENT"; + case DataID::VFAS: return "VFAS"; + } + + return "UNKNOWN"; +} diff --git a/libraries/SITL/SIM_Frsky.h b/libraries/SITL/SIM_Frsky.h new file mode 100644 index 0000000000..931173dd1b --- /dev/null +++ b/libraries/SITL/SIM_Frsky.h @@ -0,0 +1,63 @@ +/* + 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 serial rangefinders +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +#include "SIM_SerialDevice.h" + +namespace SITL { + +class Frsky : public SerialDevice { +public: + + Frsky() {}; + + // update state + virtual void update() = 0; + +protected: + + enum class DataID { + GPS_ALT_BP = 0x01, + TEMP1 = 0x02, + FUEL = 0x04, + TEMP2 = 0x05, + GPS_ALT_AP = 0x09, + BARO_ALT_BP = 0x10, + GPS_SPEED_BP = 0x11, + GPS_LONG_BP = 0x12, + GPS_LAT_BP = 0x13, + GPS_COURS_BP = 0x14, + GPS_SPEED_AP = 0x19, + GPS_LONG_AP = 0x1A, + GPS_LAT_AP = 0x1B, + BARO_ALT_AP = 0x21, + GPS_LONG_EW = 0x22, + GPS_LAT_NS = 0x23, + CURRENT = 0x28, + VFAS = 0x39, + }; + + const char *dataid_string(DataID id); +}; + +} diff --git a/libraries/SITL/SIM_Frsky_D.cpp b/libraries/SITL/SIM_Frsky_D.cpp new file mode 100644 index 0000000000..fa06c236cf --- /dev/null +++ b/libraries/SITL/SIM_Frsky_D.cpp @@ -0,0 +1,108 @@ +/* + 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 FrSky D telemetery +*/ + +#include "SIM_Frsky_D.h" + +#include + +using namespace SITL; + +// sadly, this pulls START_STOP_D etc in from the frsky header. +#include + +// const uint8_t START_STOP_D = 0x5E; +// const uint8_t BYTESTUFF_D = 0x5D; + +void Frsky_D::handle_data(uint8_t id, uint16_t data) +{ + ::fprintf(stderr, + "Frsky: id=%s (0x%02X) data=%u\n", + dataid_string((DataID)id), + (unsigned)_id, + (unsigned)data); +} + +void Frsky_D::update() +{ + const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1); + if (n != -1) { + _buflen += n; + } + + if (_buflen == 0) { + return; + } + + while (_buflen) { + switch (_state) { + case State::WANT_START_STOP_D: + if (_buffer[0] != START_STOP_D) { + AP_HAL::panic("Corrupt?"); + // _lost_bytes++; + continue; + } + memcpy(&_buffer[0], &_buffer[1], --_buflen); //srsly?! + _state = State::WANT_ID; + break; + case State::WANT_ID: + _id = _buffer[0]; + memcpy(&_buffer[0], &_buffer[1], --_buflen); //srsly?! + _state = State::WANT_BYTE1; + break; + case State::WANT_BYTE1: + case State::WANT_BYTE2: { + uint8_t byte; + uint8_t consume = 1; + if (_buffer[0] == 0x5D) { + // byte-stuffed + if (_buflen < 2) { + return; + } + if (_buffer[1] == 0x3E) { + byte = START_STOP_D; + } else if (_buffer[1] == 0x3D) { + byte = BYTESTUFF_D; + } else { + AP_HAL::panic("Unknown stuffed byte"); + } + consume = 2; + } else { + byte = _buffer[0]; + } + + memcpy(&_buffer[0], &_buffer[consume], _buflen-consume); + _buflen -= consume; + + switch (_state) { + case State::WANT_ID: + case State::WANT_START_STOP_D: + AP_HAL::panic("Should not get here"); + case State::WANT_BYTE1: + _data = byte; + _state = State::WANT_BYTE2; + break; + case State::WANT_BYTE2: + _data |= byte << 8; + handle_data(_id, _data); + _state = State::WANT_START_STOP_D; + break; + } + } + } + } +} diff --git a/libraries/SITL/SIM_Frsky_D.h b/libraries/SITL/SIM_Frsky_D.h new file mode 100644 index 0000000000..5d5c8fa96a --- /dev/null +++ b/libraries/SITL/SIM_Frsky_D.h @@ -0,0 +1,68 @@ +/* + 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 . + */ +/* + Simulated Frsky D device + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:frsky-d --speedup=1 + +param set SERIAL5_PROTOCOL 3 +reboot + +arm throttle +rc 3 1600 + +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +#include "SIM_Frsky.h" + +namespace SITL { + +class Frsky_D : public Frsky { +public: + + using Frsky::Frsky; + + // update state + virtual void update() override; + +private: + + enum class State { + WANT_START_STOP_D = 16, + WANT_ID = 17, + WANT_BYTE1 = 18, + WANT_BYTE2 = 19, + }; + State _state = State::WANT_START_STOP_D; + + char _buffer[32]; + uint16_t _buflen = 0; + + uint8_t _id; + uint16_t _data; + + + void handle_data(uint8_t id, uint16_t data); + + void shift_start_stop_d(); +}; + +}