Skip to content

add RDY ready-to-arm sensor #308

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ramiss wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add RDY ready-to-arm sensor #308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 50 additions & 0 deletions OTX_ETX/bw128x64/SRC/yaapu7.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -221,6 +221,9 @@ status.showMinMaxValues = false
status.terrainLastData = getTime()
status.terrainEnabled = 0
status.airspeedEnabled = 0
-- READY TO ARM (RDY sensor)
status.readyToArm = false
status.lastPassthroughTime = 0

-- 00 05 10 15 20 25 30 40 50 60 70 80 90
-- MIN MAX
Expand Down Expand Up @@ -563,6 +566,11 @@ local function reset()
noTelemetryData = 1
hideNoTelemetry = false
---------------
-- READY TO ARM (RDY sensor)
---------------
status.readyToArm = false
status.lastPassthroughTime = 0
---------------
-- FLIGHT TIME
---------------
lastTimerStart = 0
Expand Down Expand Up @@ -655,7 +663,20 @@ local function resetHash()
hashByteIndex = 0
end

-- RDY sensor: the ready to arm evidence is scoped to the current FC power cycle
local function checkReadyToArmMessage(msg)
if string.find(msg, "ArduCopter V", 1, true) ~= nil then
-- FC boot banner, a new power cycle invalidates any previous evidence
status.readyToArm = false
elseif string.find(msg, "using GPS", 1, true) ~= nil then
-- "EKF3 IMUx is using GPS", the FC has a GPS aided EKF solution
status.readyToArm = true
end
end

local function processTelemetry(appId, value, now)
-- RDY sensor: track link freshness, a passthrough frame was just decoded
status.lastPassthroughTime = now
if appId == 0x5006 then -- ROLLPITCH
-- roll [0,1800] ==> [-180,180]
telemetry.roll = (math.min(bit32.extract(value,0,11),1800) - 900) * 0.2
Expand Down Expand Up @@ -740,6 +761,7 @@ local function processTelemetry(appId, value, now)
if msgEnd then
-- push and display message
local severity = (bit32.extract(value,7,1) * 1) + (bit32.extract(value,15,1) * 2) + (bit32.extract(value,23,1) * 4)
checkReadyToArmMessage(msgBuffer)
pushMessage( severity, msgBuffer)
playHash()
resetHash()
Expand Down Expand Up @@ -813,6 +835,7 @@ local function crossfirePop()
-- hash support
updateHash(data[i])
end
checkReadyToArmMessage(msgBuffer)
pushMessage(severity, msgBuffer)
-- hash audio support
playHash()
Expand Down Expand Up @@ -1117,7 +1140,26 @@ local function calcFlightTime()
status.flightTime = model.getTimer(2).value
end

-- RDY sensor: 100 when the FC is effectively ready to arm, 0 otherwise
local function getReadyToArmValue()
-- no recent passthrough frame, the FC is powered down or the link is gone
if status.lastPassthroughTime == 0 then
return 0
end
-- authoritative, the FC announced a GPS aided EKF solution this power cycle
if status.readyToArm == true then
return 100
end
-- fallback composite from the repeating 0x5002 GPS STATUS frame
if (telemetry.gpsStatus or 0) >= 3 and (telemetry.gpsHdopC or 100) <= 14 and (telemetry.numSats or 0) >= 10 then
return 100
end
return 0
end

local function setSensorValues()
-- RDY is published even without a live link so it can always fall back to 0
setTelemetryValue(0x060F, 0, 2, getReadyToArmValue(), 0 , 0 , "RDY")
if not telemetryEnabled() then
return
end
Expand Down Expand Up @@ -1416,6 +1458,14 @@ local function background()
end
-- SLOW: this runs at 4Hz (every 250ms)
if (bgclock % 4 == 0) then
-- RDY sensor: expire the ready to arm state 8s after the last passthrough frame
if status.lastPassthroughTime ~= 0 then
local elapsed = now - status.lastPassthroughTime
if elapsed > 800 or elapsed < 0 then
status.readyToArm = false
status.lastPassthroughTime = 0
end
end
setSensorValues()
updateTotalDist(now)
end
Expand Down
50 changes: 50 additions & 0 deletions OTX_ETX/bw212x64_f2/SRC/yaapu9.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,9 @@ status.flightTime = 0 -- updated from model timer 3
status.timerRunning = 0 -- triggered by landcomplete from AP
status.showMinMaxValues = false
status.airspeedEnabled = 0
-- READY TO ARM (RDY sensor)
status.readyToArm = false
status.lastPassthroughTime = 0

-- 00 05 10 15 20 25 30 40 50 60 70 80 90
-- MIN MAX
Expand Down Expand Up @@ -551,6 +554,11 @@ local function reset()
noTelemetryData = 1
hideNoTelemetry = false
---------------
-- READY TO ARM (RDY sensor)
---------------
status.readyToArm = false
status.lastPassthroughTime = 0
---------------
-- FLIGHT TIME
---------------
lastTimerStart = 0
Expand Down Expand Up @@ -643,7 +651,20 @@ local function resetHash()
hashByteIndex = 0
end

-- RDY sensor: the ready to arm evidence is scoped to the current FC power cycle
local function checkReadyToArmMessage(msg)
if string.find(msg, "ArduCopter V", 1, true) ~= nil then
-- FC boot banner, a new power cycle invalidates any previous evidence
status.readyToArm = false
elseif string.find(msg, "using GPS", 1, true) ~= nil then
-- "EKF3 IMUx is using GPS", the FC has a GPS aided EKF solution
status.readyToArm = true
end
end

local function processTelemetry(appId, value, now)
-- RDY sensor: track link freshness, a passthrough frame was just decoded
status.lastPassthroughTime = now
if appId == 0x5006 then -- ROLLPITCH
-- roll [0,1800] ==> [-180,180]
telemetry.roll = (math.min(bit32.extract(value,0,11),1800) - 900) * 0.2
Expand Down Expand Up @@ -726,6 +747,7 @@ local function processTelemetry(appId, value, now)
if msgEnd then
-- push and display message
local severity = (bit32.extract(value,7,1) * 1) + (bit32.extract(value,15,1) * 2) + (bit32.extract(value,23,1) * 4)
checkReadyToArmMessage(msgBuffer)
pushMessage( severity, msgBuffer)
playHash()
resetHash()
Expand Down Expand Up @@ -788,6 +810,7 @@ local function crossfirePop()
-- hash support
updateHash(data[i])
end
checkReadyToArmMessage(msgBuffer)
pushMessage(severity, msgBuffer)
-- hash audio support
playHash()
Expand Down Expand Up @@ -1086,7 +1109,26 @@ local function calcFlightTime()
status.flightTime = model.getTimer(2).value
end

-- RDY sensor: 100 when the FC is effectively ready to arm, 0 otherwise
local function getReadyToArmValue()
-- no recent passthrough frame, the FC is powered down or the link is gone
if status.lastPassthroughTime == 0 then
return 0
end
-- authoritative, the FC announced a GPS aided EKF solution this power cycle
if status.readyToArm == true then
return 100
end
-- fallback composite from the repeating 0x5002 GPS STATUS frame
if (telemetry.gpsStatus or 0) >= 3 and (telemetry.gpsHdopC or 100) <= 14 and (telemetry.numSats or 0) >= 10 then
return 100
end
return 0
end

local function setSensorValues()
-- RDY is published even without a live link so it can always fall back to 0
setTelemetryValue(0x060F, 0, 2, getReadyToArmValue(), 0 , 0 , "RDY")
if not telemetryEnabled() then
return
end
Expand Down Expand Up @@ -1380,6 +1422,14 @@ local function background()
end
-- SLOW: this runs at 4Hz (every 250ms)
if (bgclock % 4 == 0) then
-- RDY sensor: expire the ready to arm state 8s after the last passthrough frame
if status.lastPassthroughTime ~= 0 then
local elapsed = now - status.lastPassthroughTime
if elapsed > 800 or elapsed < 0 then
status.readyToArm = false
status.lastPassthroughTime = 0
end
end
setSensorValues()
updateTotalDist(now)
end
Expand Down
50 changes: 50 additions & 0 deletions OTX_ETX/bw212x64_f4/SRC/yaapu9.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -221,6 +221,9 @@ status.showMinMaxValues = false
status.terrainLastData = getTime()
status.terrainEnabled = 0
status.airspeedEnabled = 0
-- READY TO ARM (RDY sensor)
status.readyToArm = false
status.lastPassthroughTime = 0

-- 00 05 10 15 20 25 30 40 50 60 70 80 90
-- MIN MAX
Expand Down Expand Up @@ -563,6 +566,11 @@ local function reset()
noTelemetryData = 1
hideNoTelemetry = false
---------------
-- READY TO ARM (RDY sensor)
---------------
status.readyToArm = false
status.lastPassthroughTime = 0
---------------
-- FLIGHT TIME
---------------
lastTimerStart = 0
Expand Down Expand Up @@ -655,7 +663,20 @@ local function resetHash()
hashByteIndex = 0
end

-- RDY sensor: the ready to arm evidence is scoped to the current FC power cycle
local function checkReadyToArmMessage(msg)
if string.find(msg, "ArduCopter V", 1, true) ~= nil then
-- FC boot banner, a new power cycle invalidates any previous evidence
status.readyToArm = false
elseif string.find(msg, "using GPS", 1, true) ~= nil then
-- "EKF3 IMUx is using GPS", the FC has a GPS aided EKF solution
status.readyToArm = true
end
end

local function processTelemetry(appId, value, now)
-- RDY sensor: track link freshness, a passthrough frame was just decoded
status.lastPassthroughTime = now
if appId == 0x5006 then -- ROLLPITCH
-- roll [0,1800] ==> [-180,180]
telemetry.roll = (math.min(bit32.extract(value,0,11),1800) - 900) * 0.2
Expand Down Expand Up @@ -740,6 +761,7 @@ local function processTelemetry(appId, value, now)
if msgEnd then
-- push and display message
local severity = (bit32.extract(value,7,1) * 1) + (bit32.extract(value,15,1) * 2) + (bit32.extract(value,23,1) * 4)
checkReadyToArmMessage(msgBuffer)
pushMessage( severity, msgBuffer)
playHash()
resetHash()
Expand Down Expand Up @@ -813,6 +835,7 @@ local function crossfirePop()
-- hash support
updateHash(data[i])
end
checkReadyToArmMessage(msgBuffer)
pushMessage(severity, msgBuffer)
-- hash audio support
playHash()
Expand Down Expand Up @@ -1117,7 +1140,26 @@ local function calcFlightTime()
status.flightTime = model.getTimer(2).value
end

-- RDY sensor: 100 when the FC is effectively ready to arm, 0 otherwise
local function getReadyToArmValue()
-- no recent passthrough frame, the FC is powered down or the link is gone
if status.lastPassthroughTime == 0 then
return 0
end
-- authoritative, the FC announced a GPS aided EKF solution this power cycle
if status.readyToArm == true then
return 100
end
-- fallback composite from the repeating 0x5002 GPS STATUS frame
if (telemetry.gpsStatus or 0) >= 3 and (telemetry.gpsHdopC or 100) <= 14 and (telemetry.numSats or 0) >= 10 then
return 100
end
return 0
end

local function setSensorValues()
-- RDY is published even without a live link so it can always fall back to 0
setTelemetryValue(0x060F, 0, 2, getReadyToArmValue(), 0 , 0 , "RDY")
if not telemetryEnabled() then
return
end
Expand Down Expand Up @@ -1416,6 +1458,14 @@ local function background()
end
-- SLOW: this runs at 4Hz (every 250ms)
if (bgclock % 4 == 0) then
-- RDY sensor: expire the ready to arm state 8s after the last passthrough frame
if status.lastPassthroughTime ~= 0 then
local elapsed = now - status.lastPassthroughTime
if elapsed > 800 or elapsed < 0 then
status.readyToArm = false
status.lastPassthroughTime = 0
end
end
setSensorValues()
updateTotalDist(now)
end
Expand Down
Loading