aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: add simulated Frsky devices

Browse Source
This commit is contained in:
Peter Barker 2019-12-17 13:22:13 +11:00 committed by Peter Barker
parent 0c8e6f212d
commit 2f60f783d4
4 changed files with 286 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/SITL/SIM_Frsky.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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";
}

63
libraries/SITL/SIM_Frsky.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
Base class for serial rangefinders
*/
#pragma once
#include "SIM_Aircraft.h"
#include <SITL/SITL.h>
#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);
};
}

108
libraries/SITL/SIM_Frsky_D.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
Base class for FrSky D telemetery
*/
#include "SIM_Frsky_D.h"
#include <stdio.h>
using namespace SITL;
// sadly, this pulls START_STOP_D etc in from the frsky header.
#include <GCS_MAVLink/GCS.h>
// 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;
}
}
}
}
}

68
libraries/SITL/SIM_Frsky_D.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <SITL/SITL.h>
#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();
};
}