diff --git a/libraries/SITL/SIM_RF_NMEA.h b/libraries/SITL/SIM_RF_NMEA.h index 7a667a7fdc..d9dbb8079b 100644 --- a/libraries/SITL/SIM_RF_NMEA.h +++ b/libraries/SITL/SIM_RF_NMEA.h @@ -18,6 +18,7 @@ ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:nmea --speedup=1 param set SERIAL5_PROTOCOL 9 +param set SERIAL5_BAUD 9600 param set RNGFND1_TYPE 17 graph RANGEFINDER.distance graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance @@ -36,6 +37,8 @@ namespace SITL { class RF_NMEA : public SerialRangeFinder { public: + uint32_t device_baud() const override { return 9600; } + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; bool has_temperature() const override { return true; } diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index cf442083dc..d4f798aabf 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -64,6 +64,10 @@ ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size) const { + if (device_baud() != 0 && autopilot_baud != 0 && device_baud() != autopilot_baud) { + return -1; + } + const ssize_t ret = to_autopilot->write((uint8_t*)buffer, size); // ::fprintf(stderr, "write to autopilot: ("); // for (ssize_t i=0; iread((uint8_t*)buffer, size); return ret; } diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index cdfe5637f2..6dba244061 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -32,10 +32,12 @@ public: // methods for autopilot to use to talk to device: ssize_t read_from_device(char *buffer, size_t size) const; ssize_t write_to_device(const char *buffer, size_t size) const; + void set_autopilot_baud(uint32_t baud) { autopilot_baud = baud; } // methods for simulated device to use: ssize_t read_from_autopilot(char *buffer, size_t size) const; virtual ssize_t write_to_autopilot(const char *buffer, size_t size) const; + virtual uint32_t device_baud() const { return 0; } // 0 meaning unset protected: @@ -45,6 +47,10 @@ protected: ByteBuffer *from_autopilot; bool init_sitl_pointer(); + +private: + + uint32_t autopilot_baud; }; }