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

SITL: add support for simulated serial rangefinders

Browse Source 
SITL: add base class for serial rangefinder simulators

SITL: add Benewake rangefinder simulator

SITL: add support for simulated LightWareSerial rangefinder

SITL: add support for simulated Lanbao rangefinder

SITL: add support for simulated BLping rangefinder

SITL: add support for simulated LeddarOne rangefinder

SITL: add support for simulated uLanding rangefinders

SITL: add support for simulated MaxsonarSerialLV rangefinders

SITL: add support for simulated Wasp rangefinders

SITL: add support for simulated NMEA rangefinders
This commit is contained in:
Peter Barker 2019-11-07 15:03:44 +11:00 committed by Peter Barker
parent 990273a778
commit df0233a7d1
27 changed files with 1237 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/SITL/SIM_RF_BLping.cpp Normal file
View File

@ -0,0 +1,66 @@
/*
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/>.
*/
/*
Simulator for the BLping rangefinder
*/
#include "SIM_RF_BLping.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
using namespace SITL;
uint32_t RF_BLping::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
#define BLPING_MSGID_ACK 1
#define BLPING_MSGID_NACK 2
#define BLPING_MSGID_SET_PING_INTERVAL 1004
#define BLPING_MSGID_GET_DEVICE_ID 1201
#define BLDPIN_MSGID_DISTANCE_SIMPLE 1211
#define BLPING_MSGID_CONTINUOUS_START 1400
const uint32_t alt_mm = uint32_t(alt_cm * 10);
const uint8_t payload[] = {
uint8_t(alt_mm & 0xff),
uint8_t((alt_mm >> 8) & 0xff),
uint8_t((alt_mm >> 16) & 0xff),
uint8_t((alt_mm >> 24) & 0xff),
};
const uint16_t message_id = BLDPIN_MSGID_DISTANCE_SIMPLE;
const uint8_t src_device_id = 1;
const uint8_t dst_device_id = 0;
uint16_t offs = 0;
buffer[offs++] = 0x42;
buffer[offs++] = 0x52;
buffer[offs++] = ARRAY_SIZE(payload) & 0xff;
buffer[offs++] = ARRAY_SIZE(payload) >> 8;
buffer[offs++] = message_id & 0xff;
buffer[offs++] = message_id >> 8;
buffer[offs++] = src_device_id;
buffer[offs++] = dst_device_id;
memcpy(&buffer[offs], payload, ARRAY_SIZE(payload));
offs += ARRAY_SIZE(payload);
uint16_t crc = 0;
for (uint8_t i=0; i<offs; i++) {
crc += buffer[i];
}
buffer[offs++] = crc & 0xff;
buffer[offs++] = crc >> 8;
return offs;
}

38
libraries/SITL/SIM_RF_BLping.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the BLping rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --uartF=sim:blping --speedup=1 -l 33.810313,-118.393867,0,185
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 23
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_BLping : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

40
libraries/SITL/SIM_RF_Benewake.cpp Normal file
View File

@ -0,0 +1,40 @@
/*
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 simulator for the Benewake Serial RangeFinders
*/
#include "SIM_RF_Benewake.h"
using namespace SITL;
uint32_t RF_Benewake::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
buffer[0] = 0x59;
buffer[1] = 0x59;
buffer[3] = alt_cm >> 8;
buffer[2] = alt_cm & 0xff;
buffer[4] = byte4();
buffer[5] = byte5();
buffer[6] = byte6();
buffer[7] = byte7();
// calculate checksum:
buffer[8] = 0;
for (uint8_t i=0; i<8; i++) {
buffer[8] += buffer[i];
}
return 9;
}

39
libraries/SITL/SIM_RF_Benewake.h Normal file
View File

@ -0,0 +1,39 @@
/*
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 simulator for the Benewake Serial RangeFinders
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_Benewake : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
private:
virtual uint8_t byte4() const = 0;
virtual uint8_t byte5() const = 0;
virtual uint8_t byte6() const = 0;
virtual uint8_t byte7() const = 0;
};
}

43
libraries/SITL/SIM_RF_Benewake_TF02.h Normal file
View File

@ -0,0 +1,43 @@
/*
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/>.
*/
/*
Simulator for the Benewake TF02 RangeFinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf02 --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 19
reboot
*/
#pragma once
#include "SIM_RF_Benewake.h"
namespace SITL {
class RF_Benewake_TF02 : public RF_Benewake {
public:
// see AP_RangeFinder_Benewake.cpp for definitions
uint8_t byte4() const override { return 1; } // strength low-bits
uint8_t byte5() const override { return 1; } // strength high-bits
uint8_t byte6() const override { return 7; } // reliability
uint8_t byte7() const override { return 0x06; } // exposure time
};
}

43
libraries/SITL/SIM_RF_Benewake_TF03.h Normal file
View File

@ -0,0 +1,43 @@
/*
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/>.
*/
/*
Simulator for the Benewake TF03 RangeFinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf03 --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 27
reboot
*/
#pragma once
#include "SIM_RF_Benewake.h"
namespace SITL {
class RF_Benewake_TF03 : public RF_Benewake {
public:
// see AP_RangeFinder_Benewake.cpp for definitions
uint8_t byte4() const override { return 0; } // reserved
uint8_t byte5() const override { return 0; } // reserved
uint8_t byte6() const override { return 0; } // reserved
uint8_t byte7() const override { return 0; } // TF02 only
};
}

43
libraries/SITL/SIM_RF_Benewake_TFmini.h Normal file
View File

@ -0,0 +1,43 @@
/*
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/>.
*/
/*
Simulator for the TFMini RangeFinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tfmini --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 20
reboot
*/
#pragma once
#include "SIM_RF_Benewake.h"
namespace SITL {
class RF_Benewake_TFmini : public RF_Benewake {
public:
// see AP_RangeFinder_Benewake.cpp for definitions
uint8_t byte4() const override { return 1; } // strength L
uint8_t byte5() const override { return 1; } // strength H
uint8_t byte6() const override { return 0x07; } // distance mode (0x02=mm 0x07=cm)
uint8_t byte7() const override { return 0; } // TF02 only
};
}

39
libraries/SITL/SIM_RF_Lanbao.cpp Normal file
View File

@ -0,0 +1,39 @@
/*
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/>.
*/
/*
Simulator for the Lanbao rangefinder
*/
#include "SIM_RF_Lanbao.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
using namespace SITL;
uint32_t RF_Lanbao::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
buffer[0] = 0xA5;
buffer[1] = 0x5A;
buffer[2] = (alt_cm * 10) >> 8;
buffer[3] = (alt_cm * 10) & 0xff;
const uint16_t crc = calc_crc_modbus(buffer, 4);
buffer[4] = crc & 0xff;
buffer[5] = crc >> 8;
return 6;
}

38
libraries/SITL/SIM_RF_Lanbao.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the Lanbao rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lanbao --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 26
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_Lanbao : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

68
libraries/SITL/SIM_RF_LeddarOne.cpp 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/>.
*/
/*
Simulator for the LeddarOne rangefinder
*/
#include "SIM_RF_LeddarOne.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
using namespace SITL;
uint32_t RF_LeddarOne::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
const uint8_t response_size = 25;
const uint16_t internal_temperature = 1245;
const uint16_t num_detections = 1;
const uint16_t first_distance = alt_cm * 10;
const uint16_t first_amplitude = 37;
const uint16_t second_distance = alt_cm * 10;
const uint16_t second_amplitude = 37;
const uint16_t third_distance = alt_cm * 10;
const uint16_t third_amplitude = 37;
const uint32_t now = AP_HAL::millis();
buffer[0] = 0x01;
buffer[1] = 0x04; // magic function number! LEDDARONE_MODOBUS_FUNCTION_CODE
buffer[2] = response_size;
buffer[3] = (now >> 16) & 0xff;
buffer[4] = (now >> 24) & 0xff;
buffer[5] = (now >> 0) & 0xff;
buffer[6] = (now >> 8) & 0xff;
buffer[7] = internal_temperature & 0xff;
buffer[8] = internal_temperature >> 8;
buffer[9] = num_detections >> 8;
buffer[10] = num_detections & 0xff;
buffer[11] = first_distance >> 8;
buffer[12] = first_distance & 0xff;
buffer[13] = first_amplitude >> 8;
buffer[14] = first_amplitude & 0xff;
buffer[15] = second_distance >> 8;
buffer[16] = second_distance & 0xff;
buffer[17] = second_amplitude >> 8;
buffer[18] = second_amplitude & 0xff;
buffer[19] = third_distance >> 8;
buffer[20] = third_distance & 0xff;
buffer[21] = third_amplitude >> 8;
buffer[22] = third_amplitude & 0xff;
const uint16_t crc = calc_crc_modbus(buffer, 23);
buffer[23] = crc & 0xff;
buffer[24] = crc >> 8;
return response_size;
}

38
libraries/SITL/SIM_RF_LeddarOne.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the LeddarOne rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:leddarone --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 12
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_LeddarOne : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

54
libraries/SITL/SIM_RF_LightWareSerial.cpp Normal file
View File

@ -0,0 +1,54 @@
/*
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/>.
*/
/*
Simulator for the serial LightWare rangefinder
*/
#include "SIM_RF_LightWareSerial.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
using namespace SITL;
bool RF_LightWareSerial::check_synced()
{
if (!synced) {
// just try to slurp a buffer in one hit:
char buffer[12] {};
ssize_t n = read_from_autopilot(buffer, ARRAY_SIZE(buffer) - 1);
if (n > 0) {
if (!strncmp(buffer, "www\r\n", ARRAY_SIZE(buffer))) {
gcs().send_text(MAV_SEVERITY_INFO, "Slurped a sync thing\n");
synced = true;
}
}
}
return synced;
}
void RF_LightWareSerial::update(float range)
{
if (!check_synced()) {
return;
}
return SerialRangeFinder::update(range);
}
uint32_t RF_LightWareSerial::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
return snprintf((char*)buffer, buflen, "%f\r", alt_cm / 100.0f); // note tragic lack of snprintf return checking
}

44
libraries/SITL/SIM_RF_LightWareSerial.h Normal file
View File

@ -0,0 +1,44 @@
/*
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/>.
*/
/*
Simulator for the serial LightWare rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lightwareserial --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 8
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_LightWareSerial : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
void update(float range) override;
private:
bool check_synced();
bool synced;
};
}

30
libraries/SITL/SIM_RF_MaxsonarSerialLV.cpp Normal file
View File

@ -0,0 +1,30 @@
/*
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/>.
*/
/*
Simulator for the Maxsonar Serial LV rangefinder
*/
#include "SIM_RF_MaxsonarSerialLV.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
using namespace SITL;
uint32_t RF_MaxsonarSerialLV::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
const float inches = alt_cm / 2.54f;
return snprintf((char*)buffer, buflen, "%u\r", (unsigned)inches);
}

38
libraries/SITL/SIM_RF_MaxsonarSerialLV.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the MaxsonarSerialLV rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:maxsonarseriallv --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 13
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_MaxsonarSerialLV : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

40
libraries/SITL/SIM_RF_NMEA.cpp Normal file
View File

@ -0,0 +1,40 @@
/*
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/>.
*/
/*
Simulator for the NMEA Serial rangefinder
*/
#include "SIM_RF_NMEA.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include <string.h>
using namespace SITL;
uint32_t RF_NMEA::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
// Format 2 DBT NMEA mode (e.g. $SMDBT,5.94,f,1.81,M,67)
// Format 3 DPT NMEA mode (e.g. $SMDPT,1.81,0.066)
ssize_t ret = snprintf((char*)buffer, buflen, "$SMDPT,%f,%f", alt_cm/100.0f, 0.01f);
uint8_t checksum = 0;
for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped
checksum ^= buffer[i];
}
ret += snprintf((char*)&buffer[ret], buflen-ret, "*%02X\r\n", checksum);
return ret;
}

40
libraries/SITL/SIM_RF_NMEA.h Normal file
View File

@ -0,0 +1,40 @@
/*
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/>.
*/
/*
Simulator for the NMEA serial rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:nmea --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 17
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_NMEA : public SerialRangeFinder {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
private:
};
}

117
libraries/SITL/SIM_RF_Wasp.cpp Normal file
View File

@ -0,0 +1,117 @@
/*
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/>.
*/
/*
Simulator for the Wasp Serial rangefinder
*/
#include "SIM_RF_Wasp.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include <string.h>
using namespace SITL;
void RF_Wasp::check_configuration()
{
const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
if (n <= 0) {
return;
}
_buflen += n;
// ensure we have an entire line:
const char *cr = strchr(_buffer, '\n');
if (cr == nullptr) {
if (_buflen == ARRAY_SIZE(_buffer) - 1) {
// nuke it all
memset(_buffer, '\0', ARRAY_SIZE(_buffer));
_buflen = 0;
}
return;
}
if (!strncmp(_buffer, ">GO\n", _buflen)) {
config.go = true;
const char *response = "GO\n";
write_to_autopilot(response, strlen(response));
} else if (_buffer[0] == '>') {
bool set = false;
if (!set) {
// check for string settings
for (uint8_t i=0; i<ARRAY_SIZE(string_configs); i++) {
if (!strncmp(&_buffer[1], string_configs[i].name, strlen(string_configs[i].name))) {
uint8_t offs = strlen(string_configs[i].name);
offs += 1; // for '>'
offs += 1; // for space
strncpy(string_configs[i].value, &_buffer[offs], MIN(ARRAY_SIZE(config.format), unsigned(cr - _buffer - offs - 1))); // -1 for the lf, -1 for the cr
// gcs().send_text(MAV_SEVERITY_INFO, "Wasp: config (%s) (%s)", string_configs[i].name, string_configs[i].value);
char response[128];
const size_t x = snprintf(response,
ARRAY_SIZE(response),
"%s %s\n",
string_configs[i].name,
string_configs[i].value);
write_to_autopilot(response, x);
set = true;
break;
}
}
}
if (!set) {
// check for integer settings
for (uint8_t i=0; i<ARRAY_SIZE(integer_configs); i++) {
if (!strncmp(&_buffer[1], integer_configs[i].name, strlen(integer_configs[i].name))) {
uint8_t offs = strlen(integer_configs[i].name);
offs += 1; // for '>'
offs += 1; // for space
char tmp[32]{};
strncpy(tmp, &_buffer[offs], MIN(ARRAY_SIZE(config.format), unsigned(cr - _buffer - offs - 1))); // -1 for the lf, -1 for the cr
*(integer_configs[i].value) = atoi(tmp);
// gcs().send_text(MAV_SEVERITY_INFO, "Wasp: config (%s) (%d)", integer_configs[i].name, *(integer_configs[i].value));
char response[128];
const size_t x = snprintf(response,
ARRAY_SIZE(response),
"%s %d\n",
integer_configs[i].name,
*(integer_configs[i].value));
write_to_autopilot(response, x);
set = true;
break;
}
}
}
if (!set) {
gcs().send_text(MAV_SEVERITY_INFO, "Wasp: unknown setting (%s)", &_buffer[0]);
}
}
// just nuke everything in the buffer, not just what we just
// processed. This is until we sort out the extra-cr thing
memset(_buffer, '\0', ARRAY_SIZE(_buffer));
_buflen = 0;
}
void RF_Wasp::update(float range)
{
check_configuration();
return SerialRangeFinder::update(range);
}
uint32_t RF_Wasp::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
return snprintf((char*)buffer, buflen, "%f\n", alt_cm/100.0f);
}

80
libraries/SITL/SIM_RF_Wasp.h Normal file
View File

@ -0,0 +1,80 @@
/*
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/>.
*/
/*
Simulator for the Wasp serial rangefinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:wasp --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 18
reboot
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_Wasp : public SerialRangeFinder {
public:
void update(float range) override;
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
private:
void check_configuration();
struct {
bool go;
char format[16]; // e.g. ASCII
char baud[5]; // low or high
char lbe[7]; // big or little
int frq;
int aut;
int mavg;
int medf;
int avg;
int auv;
} config;
const struct {
const char *name;
char *value;
} string_configs[3] {
{ "FMT", config.format },
{ "BAUD", config.baud },
{ "LBE", config.lbe },
};
const struct {
const char *name;
int *value;
} integer_configs[6] {
{ "FRQ", &config.frq },
{ "AUT", &config.aut },
{ "MAVG", &config.mavg },
{ "MEDF", &config.medf },
{ "AVG", &config.avg },
{ "AUV", &config.auv },
};
char _buffer[256]; // from-autopilot
uint8_t _buflen;
};
}

0
libraries/SITL/SIM_RF_uLanding.cpp Normal file
View File

29
libraries/SITL/SIM_RF_uLanding.h Normal file
View File

@ -0,0 +1,29 @@
/*
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 simulator for the Benewake Serial RangeFinders
*/
#pragma once
#include "SIM_SerialRangeFinder.h"
namespace SITL {
class RF_uLanding : public SerialRangeFinder {
public:
};
}

36
libraries/SITL/SIM_RF_uLanding_v0.cpp Normal file
View File

@ -0,0 +1,36 @@
/*
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/>.
*/
/*
Simulator for the uLanding v0 rangefinder
*/
#include "SIM_RF_uLanding_v0.h"
using namespace SITL;
uint32_t RF_uLanding_v0::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
const uint16_t reading = alt_cm / 2.5f;
buffer[0] = 0x48;
buffer[1] = reading & 0x7f;
buffer[2] = (reading >> 7) & 0xff;
// the detection routine is crap, frankly. Needs lots of bytes
// *in one read* to work.
buffer[3] = 0x48;
buffer[4] = reading & 0x7f;
buffer[5] = (reading >> 7) & 0xff;
return 6;
}

38
libraries/SITL/SIM_RF_uLanding_v0.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the uLanding v0 Serial RangeFinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ulanding_v0 --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 11
reboot
*/
#pragma once
#include "SIM_RF_uLanding.h"
namespace SITL {
class RF_uLanding_v0 : public RF_uLanding {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

34
libraries/SITL/SIM_RF_uLanding_v1.cpp Normal file
View File

@ -0,0 +1,34 @@
/*
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/>.
*/
/*
Simulator for the uLanding v1 rangefinder
*/
#include "SIM_RF_uLanding_v1.h"
using namespace SITL;
uint32_t RF_uLanding_v1::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
buffer[0] = 0xFE;
buffer[1] = 0; // unused?
buffer[2] = alt_cm & 0xff;
buffer[3] = alt_cm >> 8;
buffer[4] = 0; // unused?
// checksum:
buffer[5] = buffer[1] + buffer[2] + buffer[3] + buffer[4];
return 6;
}

38
libraries/SITL/SIM_RF_uLanding_v1.h Normal file
View File

@ -0,0 +1,38 @@
/*
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/>.
*/
/*
Simulator for the uLanding v1 Serial RangeFinder
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ulanding_v1 --speedup=1
param set SERIAL5_PROTOCOL 9
param set RNGFND1_TYPE 11
reboot
*/
#pragma once
#include "SIM_RF_uLanding.h"
namespace SITL {
class RF_uLanding_v1 : public RF_uLanding {
public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
};
}

77
libraries/SITL/SIM_SerialRangeFinder.cpp Normal file
View File

@ -0,0 +1,77 @@
/*
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
*/
#include "SIM_SerialRangeFinder.h"
using namespace SITL;
uint16_t SerialRangeFinder::calculate_range_cm(float range_value) const
{
// swiped from sitl_rangefinder.cpp - we should unify them at some stage
const SITL *sitl = AP::sitl();
float altitude = range_value;
if (is_equal(range_value, -1.0f)) { // Use SITL altitude as reading by default
altitude = AP::sitl()->height_agl;
}
// sensor position offset in body frame
const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;
// adjust altitude for position of the sensor on the vehicle if position offset is non-zero
if (!relPosSensorBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame
const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor
altitude -= relPosSensorEF.z;
}
// If the attidude is non reversed for SITL OR we are using rangefinder from external simulator,
// We adjust the reading with noise, glitch and scaler as the reading is on analog port.
if ((fabs(sitl->state.rollDeg) < 90.0 && fabs(sitl->state.pitchDeg) < 90.0) || !is_equal(range_value, -1.0f)) {
if (is_equal(range_value, -1.0f)) { // disable for external reading that already handle this
// adjust for apparent altitude with roll
altitude /= cosf(radians(sitl->state.rollDeg)) * cosf(radians(sitl->state.pitchDeg));
}
// Add some noise on reading
altitude += sitl->sonar_noise * rand_float();
}
return altitude * 100.0f;
}
void SerialRangeFinder::update(float range)
{
// just send a chunk of data at 1Hz:
const uint32_t now = AP_HAL::millis();
if (now - last_sent_ms < 1000) {
return;
}
last_sent_ms = now;
uint8_t data[255];
const uint32_t packetlen = packet_for_alt(calculate_range_cm(range),
data,
ARRAY_SIZE(data));
write_to_autopilot((char*)data, packetlen);
}

47
libraries/SITL/SIM_SerialRangeFinder.h 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 serial rangefinders
*/
#pragma once
#include "SIM_Aircraft.h"
#include <SITL/SITL.h>
#include "SIM_SerialDevice.h"
namespace SITL {
class SerialRangeFinder : public SerialDevice {
public:
SerialRangeFinder() {};
// update state
virtual void update(float range);
virtual uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) = 0;
private:
uint32_t last_sent_ms;
uint16_t calculate_range_cm(float range_value) const;
};
}