-- Данный скрипт реализует полет по заданным точкам в системе позиционирования
-- Инструкция по настройке: настроить 8 канал на пульте в SwA. Поставить квадрокоптер, нажать кнопку запуска скрипта (квадрокоптер должен издать звуковой сигнал), отойти на безопасное расстояние. Переключить тумблер SwA вниз, после запуска моторов поднять тумблер вверх. Коптер начнет лететь по заданным координатам.


-- Упрощение вызова функции распаковки таблиц из модуля tabl

local unpack = table.unpack

-- Количество светодиодов на базовой плате

-- Создание порта управления светодиодами

local rc = Sensors.rc -- считывание положения тумблеров 






-- Таблица точек полетного задания в формате {x,y,z}
local points = {
        {0, 2, 1},
        {0, 1, 1},
        {0, 2, 1},
        {0, 1, 1}
}
-- Счетчик точек
local curr_point = 1

-- Функция, изменяющая цвет светодиодов и выполняющая полет к следующей точке
local function nextPoint()
    -- Текущий цвет. % - остаток от деления, # - размер таблицы. Такая конструкция использована,
    -- чтобы цвета продолжали меняться, даже если точек больше, чем цветов в таблице
   
    -- Изменение цвета светодиодов                                                         

    -- Полет к текущей точке, если её номер не больше количества заданных точек
    if(curr_point <= #points) then
        Timer.callLater(1, function()
            -- Команда полета к точке в системе позиционирования
            ap.goToLocalPoint(unpack(points[curr_point]))
            -- Инкрементация переменной текущей точки
            curr_point = curr_point + 1
        end)
    -- Посадка, если номер текущей точки больше количества заданных точек
    else
        Timer.callLater(1, function()
            -- Команда на посадку
            ap.push(Ev.MCE_LANDING)
        end)
    end
end

-- Функция обработки событий, автоматически вызывается автопилотом
function callback(event)
    -- Когда коптер поднялся на высоту взлета Flight_com_takeoffAlt, переходим к полету по точкам
    if(event == Ev.TAKEOFF_COMPLETE) then
        Timer.callLater(5, nextPoint)
    end
    -- Когда коптер достиг текущей точки, переходим к следующей
    if(event == Ev.POINT_REACHED) then
        Timer.callLater(5, nextPoint)
    end
    
   
end



-- Предстартовая подготовка

startTimer = Timer.new(1, function()
    rc_chans = table.pack(rc())
    if rc_chans[8] > 0 then -- проверка положения тумблера SwA
        curr_point = 1
        ap.push(Ev.MCE_PREFLIGHT)
        Timer.callLater(6, function() ap.push(Ev.MCE_TAKEOFF) end)
    end
end)
-- Таймер, через 2 секунды вызывающий функцию взлета
startTimer:start()