diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 9d01c51a42..27053a329c 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -9173,7 +9173,7 @@ switch value''' while len(wants): self.progress("Still wanting (%s)" % ",".join([("0x%02x" % x) for x in wants.keys()])) wants_copy = copy.copy(wants) - t2 = self.get_sim_time_cached() + t2 = self.get_sim_time() if t2 - tstart > 300: self.progress("Failed to get frsky passthrough data") self.progress("Counts of sensor_id polls sent:") @@ -9193,7 +9193,7 @@ switch value''' def test_frsky_passthrough(self): self.set_parameter("SERIAL5_PROTOCOL", 10) # serial5 is FRSky passthrough - self.set_parameter("RPM_TYPE", 1) # enable RPM output + self.set_parameter("RPM_TYPE", 10) # enable RPM output self.customise_SITL_commandline([ "--uartF=tcp:6735" # serial5 spews to localhost:6735 ]) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index b247fd5c88..15f80ace70 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -109,7 +109,8 @@ protected: static const uint8_t BYTESTUFF_D = 0x5D; // FrSky data IDs; - static const uint16_t RPM_LAST_ID = 0x050F; + static const uint16_t RPM1_ID = 0x050E; + static const uint16_t RPM2_ID = 0x050F; static const uint16_t GPS_LONG_LATI_FIRST_ID = 0x0800; static const uint16_t DIY_FIRST_ID = 0x5000; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 0449d6deab..3a1c7a3daa 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -126,9 +126,14 @@ void AP_Frsky_SPort::send(void) int32_t value; if (calc_rpm(_SPort.rpm_call, value)) { // use high numbered frsky sensor ids to leave low numbered free for externally attached physical frsky sensors - send_sport_frame(SPORT_DATA_FRAME, 1+RPM_LAST_ID-(rpm->num_sensors()-_SPort.rpm_call), value); + uint16_t id = RPM1_ID; + if (_SPort.rpm_call != 0) { + // only two sensors are currently supported + id = RPM2_ID; + } + send_sport_frame(SPORT_DATA_FRAME, id, value); } - if (++_SPort.rpm_call > (rpm->num_sensors()-1)) { + if (++_SPort.rpm_call > MIN(rpm->num_sensors()-1, 1)) { _SPort.rpm_call = 0; } }