import math
// position
input lat: Float64
input lon: Float64
// sensed velocity
input velo: Float64
// wind
input w_dir: Float64
input w_spd: Float64
// number of GPS satellites in range
input gps: UInt64
// timestamps for the offline analysis
input time: Float64
// check that the GPS module produces values frequent enough
output gps_freq_off: Bool @ 1Hz := lat.aggregate(over: 1s, using: count) < 9
trigger gps_freq_off "GPS sensor frequency < 9Hz"
// compare the sensed velocity and the velocity based on the GPS coordinates
output lon_diff: Float64 := lon - lon.offset(by: -1).defaults(to: lon)
output lat_diff: Float64 := lat - lat.offset(by: -1).defaults(to: lat)
output gps_dist: Float64 := sqrt(lon_diff * lon_diff + lat_diff * lat_diff)
output gps_velo: Float64 := gps_dist / (time - time.offset(by: -1).defaults(to: 0.0))
trigger abs(gps_velo - velo) > 0.1 "Deviation in sensed velocity and computed velocity."
// check whether we seem to be hovering
output yaw: Float64 := if lon_diff = 0.0 then 0.0 else arctan(lat_diff / lon_diff)
output head_wind: Bool @ 1Hz := (w_dir.hold().defaults(to: 0.0) - yaw.hold().defaults(to: 0.0)) < 0.2
output hovering: Bool @ 1Hz := (velo.aggregate(over_exactly: 5s, using: integral).defaults(to: 5.0) < 0.5) ∧ ¬head_wind
trigger hovering "Little movement in the last 5 seconds. Path planning hung up?"
output fast: Bool := velo > 7.0
output slowing_down := !fast && fast.offset(by: -1).defaults(to: false)
trigger gps = 3 "Few GPS satellites in range. 3 dimensional location unavailable"
trigger gps < 2 "Few GPS satellites in range. No GPS location available."
trigger w_spd > 14.0 "High wind speed"
trigger slowing_down "Slowing down."
trigger abs(gps_velo - velo) > 0.1 "Deviation in sensed velocity and computed velocity based on GPS data."