zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: add simulated Frsky devices

zr-v5.1
Peter Barker 5 years ago committed by Peter Barker
parent
0c8e6f212d
commit
2f60f783d4
4 changed files with 286 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 47
      libraries/SITL/SIM_Frsky.cpp
  2. 63
      libraries/SITL/SIM_Frsky.h
  3. 108
      libraries/SITL/SIM_Frsky_D.cpp
  4. 68
      libraries/SITL/SIM_Frsky_D.h

47
libraries/SITL/SIM_Frsky.cpp
Unescape View File

@ -0,0 +1,47 @@ @@ -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
Unescape View File

@ -0,0 +1,63 @@ @@ -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
Unescape View File

@ -0,0 +1,108 @@ @@ -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
Unescape View File

@ -0,0 +1,68 @@ @@ -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();
};
}
Write Preview
Loading…
Cancel
Save