local LOG_RATE_HZ = 1 local FILE_PREFIX = 'gps' local FILE_SUFFIX = 'csv' local MAV_SEVERITY_INFO = 6 local DateTime = {} DateTime.__index = DateTime setmetatable(DateTime, { __call = function(cls, y, m, d, h, n, s) local self = setmetatable({}, cls) self.y = y or 1970 self.m = m or 1 self.d = d or 1 self.h = h or 0 self.n = n or 0 self.s = s or 0 return self end }) function DateTime:__tostring() return string.format('%04d-%02d-%02d %02d:%02d:%05.2f', self.y, self.m, self.d, self.h, self.n, self.s) end local function Now() local GPS_LEAP_SECONDS = 18 -- as of 2016 GPS leap second correction local ms = gps:time_week_ms(0) - GPS_LEAP_SECONDS * 1000 local gps_week = gps:time_week(0) -- calculate GPS epoch day local day_of_week = math.floor((ms / 3600000 / 24):toint()) local gps_day = gps_week * 7 + day_of_week -- calculate time of day in hour:minute:seconds ms = ms - day_of_week * 3600000 * 24 local hour = math.floor((ms / 3600000):toint()) ms = ms - hour * 3600000 local minute = math.floor((ms / 60000):toint()) ms = ms - minute * 60000 -- iteratively calculate GPS year and Julian day local year = 1981 local day_of_year = gps_day - 360 -- GPS epoch had 360 days until 1981 while day_of_year >= 366 do day_of_year = day_of_year - 365 if year % 4 == 0 then day_of_year = day_of_year - 1 end year = year + 1 end -- create correct month-length table, accounting for leap year local Lengths = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 } if year % 4 == 0 then Lengths[2] = 29 end -- iteratively calculate month local month = 1 local day = day_of_year for index, length in ipairs(Lengths) do if day - length > 0 then month = index + 1 day = day - length end end return DateTime(year, month, day, hour, minute, ms:tofloat() / 1000) end local run_interval_ms = math.floor(1000 / LOG_RATE_HZ) local filename = '' function log_gps_data() if not arming:is_armed() then gcs:send_text(MAV_SEVERITY_INFO, string.format('Log Closed: %s', filename)) return standby, run_interval_ms end local file = assert(io.open(filename, 'a'), string.format('ERROR: Could not log to %s', filename)) for instance = 0, gps:num_sensors() - 1 do local location = gps:location(instance) local date_time = tostring(Now()):gmatch('%S+') -- like "split" in other languages local fields = { date_time(), -- iterator call will return date date_time(), -- and then time (UTC) instance + 1, location:lat(), -- degrees * 1e7 location:lng(), -- degrees * 1e7 location:alt() / 100, -- converts cm to m gps:ground_course(instance), -- degrees gps:ground_speed(instance), -- m/s gps:num_sats(instance), gps:status(instance) } file:write((table.concat(fields,',') .. '\n')) end file:close() return log_gps_data, run_interval_ms end function standby() if arming:is_armed() then local timestamp = tostring(Now()) -- remove special characters from timestamp timestamp = timestamp:gsub('% ', '_'):gsub('%:', ''):gsub('%..*', ''):gsub('%-', '') filename = string.format('%s_%s.%s', FILE_PREFIX, timestamp, FILE_SUFFIX) local file = assert(io.open(filename, 'w'), string.format('ERROR: Could not create %s', filename)) file:write('Date,UTC,GPSNum,Lat,Lng,Alt,Hdg,GndSpd,NSats,FixType\n') file:close() gcs:send_text(MAV_SEVERITY_INFO, string.format('GPS Logging: %s', filename)) return log_gps_data, 0 -- immediately log an entry end return standby, run_interval_ms end gcs:send_text(MAV_SEVERITY_INFO, 'GPS Logging Script Active') return standby, run_interval_ms