Lua Suivi de sujet par nacelle 2 ou 3D (portage du LUA Tracker de Nicephore)

Code:

--[[
   Tracker d’antenne LUA
   Version 1.0
   Bruno Van de Velde et les membres du forum http://frskytaranis.forumactif.org
]]

local function round(val, decimal) -- fonction arrondir
  -- non utilisée mais conserve pour l’avenir
  if (decimal) then
    return math.floor( (val * 10^decimal) + 0.5) / (10^decimal)
  else
    return math.floor(val+0.5)
  end
  
end --end function round(val, decimal)

local function map2(val, fromLow, fromHigh, toLow, toHigh)
  
  return (val - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
  
end --end function map2(val, in_min, in_max, out_min, out_max)

local function get_gps_home_info()

  local HOME_lat = 0
  local HOME_lon = 0
  
  HOME_lat     = getValue("pilot-latitude")    -- récup. LATITUDE, LONGITUDE de station de base                            
  HOME_lon     = getValue("pilot-longitude")
    
  return HOME_lat, HOME_lon  

end --end function get_gps_home_info()

local function get_gps_uav_info()
  
  local UAV_Lat = 0
  local UAV_Lon = 0
  local UAV_alt = 0
  
    UAV_Lat      = getValue("latitude")          -- récup. LATITUDE, LONGITUDE, ALTITUDE du modèle
    UAV_Lon      = getValue("longitude")  
    UAV_alt      = getValue("gps-altitude")
  
  return UAV_Lat, UAV_Lon, UAV_alt  

end --end function get_gps_uav_info()

local function calc_pan_tilt(lon1, lat1, lon2, lat2, alt)
  
  local R = 6371000.0    --rayon de la terre 6371km
  local a, tc1, c, dist, dLat, dLon, azimut, elev
  dist = 0
  
  lon1=lon1/180*math.pi
  lat1=lat1/180*math.pi
  lon2=lon2/180*math.pi
  lat2=lat2/180*math.pi
  
  a = math.atan2(math.sin(lon2-lon1)*math.cos(lat2), math.cos(lat1)*math.sin(lat2)-math.sin(lat1)*math.cos(lat2)*math.cos(lon2-lon1))
  azimut  = a*180/math.pi
  if (azimut  < 0) then
    azimut = math.floor(360 + azimut)
  end
 
-- Calcul de la distance en l'UAV et la station de base (home)
  dLat = (lat2-lat1)
  dLon = (lon2-lon1)
  a = math.sin(dLat/2) * math.sin(dLat/2) + math.sin(dLon/2) * math.sin(dLon/2) * math.cos(lat1) * math.cos(lat2)
  c = 2* math.asin(math.sqrt(a))
  dist = 6371000 * c
  elev = math.atan(alt/dist)
  elev = math.floor(elev*360/(2*math.pi))
 
  return azimut , elev, dist

end --end function calc_pan_tilt()

local function azimut_elev_to_ppm(azimut, elev)
  
  local azimut_ppm_value = 0
  local elev_ppm_value   = 0
  
  if (azimut >= 270 and azimut <= 360) or (azimut >= 0 and azimut <= 90) then
    if (azimut >= 1 and azimut <= 90) then
      azimut_ppm_value = math.floor(map2(azimut, 0, 90, 0, 1024))
    else
      azimut_ppm_value = math.floor(map2(azimut, 270, 360, -1024, 0))
    end
    elev_ppm_value = math.floor(map2(elev, 0, 90, -1024, 0))
  else
    if (azimut >= 90 and azimut <= 180) then
      azimut_ppm_value = math.floor(map2(azimut, 90, 180, -1024, 0))
    else
      azimut_ppm_value = math.floor(map2(azimut, 180, 270, 0, 1024))
    end
    elev_ppm_value = math.floor(map2(elev, 90, 0, 0, 1024))
  end
  
  return azimut_ppm_value, elev_ppm_value

end -- end azimut_elev_to_ppm()

local function run()
  
  local HOME_lat          = 0
  local HOME_lon          = 0
  local UAV_Lat           = 0
  local UAV_Lon           = 0
  local UAV_alt           = 0
  local AZIMUT            = 0
  local ELEV              = 0
  local DIST              = 0
  local AZIMUT_PPM_VALUE  = 0
  local ELEV_PPM_VALUE    = -1024
  
  HOME_lat, HOME_lon = get_gps_home_info()
  
  if (HOME_lat ~= 0) and (HOME_lon ~= 0) then
    
    UAV_Lat, UAV_Lon, UAV_alt = get_gps_uav_info()
    
    if UAV_alt < 0 then
      
      UAV_alt = 0
      
    end
    
    AZIMUT, ELEV, DIST = calc_pan_tilt(HOME_lon, HOME_lat, UAV_Lon, UAV_Lat, UAV_alt)
    
    AZIMUT_PPM_VALUE, ELEV_PPM_VALUE = azimut_elev_to_ppm(AZIMUT, ELEV)
  else
    AZIMUT_PPM_VALUE = 0
    ELEV_PPM_VALUE   = -1024
  end
  
  return AZIMUT_PPM_VALUE, ELEV_PPM_VALUE
  
end --end function run()

return { run=run, output={ "PAN", "TILT"} }


maxxhimum a écrit:

nicephore a écrit:Hello tous
Pour le suivi d'objet en lua dans mon script tracer y-a tous ce dont vous avez besoin sauf que la valeur du tilt sera à inverser puis que je suis au sol c'est le modèle que je suis
L'idéal serait d'avoir des entrées et ainsi donner la distance de départ ensuite le lua ferais le calcul par lui même
Ou encore avec les données GPS et décollage au plus près de la cible
Je vais regarder ça de plus près car c'est une super idée

Depuis mon machin truc phone

Salut. Effectivement Nicephore ton lua de tracker est adapté pour le lua de nacelle si on inverse les valeurs, cependant le suivi n'est qu'une fonction, il faut donc pouvoir l'activer et la désactiver, pour l'entrée ce n'est pas nécessaire si on active la fonction au dessus de l'objectif. Mais de mémoire un lua s'active à l'allumage, j'ai bon ?

maxxhimum a écrit:

nicephore a écrit:Hello tous
Pour le suivi d'objet en lua dans mon script tracer y-a tous ce dont vous avez besoin sauf que la valeur du tilt sera à inverser puis que je suis au sol c'est le modèle que je suis
L'idéal serait d'avoir des entrées et ainsi donner la distance de départ ensuite le lua ferais le calcul par lui même
Ou encore avec les données GPS et décollage au plus près de la cible
Je vais regarder ça de plus près car c'est une super idée

Depuis mon machin truc phone

Salut. Effectivement Nicephore ton lua de tracker est adapté pour le lua de nacelle si on inverse les valeurs, cependant le suivi n'est qu'une fonction, il faut donc pouvoir l'activer et la désactiver, pour l'entrée ce n'est pas nécessaire si on active la fonction au dessus de l'objectif. Mais de mémoire un lua s'active à l'allumage, j'ai bon ?

maxxhimum a écrit:

nicephore a écrit:Mon tracker d'antenne fonctionne depuis 2 servo 180° (pan et tilt) et donc en fonction du montage il peux être nécessaire d'inverser la course d'un ou des servo. Et ça tu le fait depuis la Taranis directecment pas utile de modifier le script.
Que veux tu dire par:
pas nécessaire si on active la fonction au dessus de l'objectif

En fait sur le principe ton tracker a pour vocation de fonctionner dès le décollage, donc il s'active à l'allumage de la taranis et s'initialise dès la première mesure reçue à l'allumage de la machine afin de commencer le tracking.

En revanche sur le principe le suivi de nacelle a pour vocation de fonctionner uniquement à l'activation de la fonction par un inter, et c'est là que ça se complique, il faut que le LUA enregistre comme point zéro la position de l'objectif, et donc dans l'idéal on se positionne audessus, on active l'inter de tracking de cible, le LUA enregistre la position et à partir de ce moment la la caméra ne quitte plus l'objectif des yeux. Quand la prise de vue est terminée on désactive l'inter et on repasse en contrôle semi automatique.

Il y a toujours une combine qui consiste à couper la radio au dessus de l'objectif et de la rallumer avant que le failsafe ne prenne la main, mais si on peut faire plus sécurisé je pense que c'est mieux

https://frskytaranis.forumactif.org/t2163-lua-suivi-de-sujet-par-nacelle-2-ou-3d-portage-du-lua-tracker-de-nicephore#24275

nicephore a écrit:ha ok je viens de comprendre tu ne parles pas du tracker mais d'un pointeur afin de la caméra reste pointée sur un objectif ...
j'ai pas pris le temps de revoir ça ... Mais:
il serait possible de faire le point GPS près de la cible, enregistrer dans 2 variables ces coordonnées, dans 2 autres variable les coordonnées GPS du modèle en vol, modifier le script en conséquence par exemple

 local HOME_lat = valeur GPS de la cible
 local HOME_lon = valeur GPS de la cible

Ou aussi d'utiliser des input en lua.

Pour que ceci soit plus parlant

HOME_lat et lon deviens CIBLE_lat et lon
ensuite dans la fonction AZIMUT, ELEV, DIST = calc_pan_tilt(CIBLE_lon, CIBLE_lat, UAV_Lon, UAV_Lat, UAV_alt)
et d'inverser le résultat de la variable
elev = math.floor(elev*360/(2*math.pi))
elev = elev * -1

Je te laisse tester ça mais d'après moi ce serait très simple de faire comme ça.

Code:

--[[
   POursuite de cible
   Version 1.0
   Bruno Van de Velde et les membres du forum http://frskytaranis.forumactif.org
]]

local function round(val, decimal) -- fonction arrondir
  -- non utilisée mais conservée pour l’avenir
  if (decimal) then
    return math.floor( (val * 10^decimal) + 0.5) / (10^decimal)
  else
    return math.floor(val+0.5)
  end
  
end --end function round(val, decimal)

local function map2(val, fromLow, fromHigh, toLow, toHigh)
  
  return (val - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
  
end --end function map2(val, in_min, in_max, out_min, out_max)

local function get_gps_CIBLE_info()

  local CIBLE_lat = 0
  local CIBLE_lon = 0
  
  CIBLE_lat     = getValue("CIBLE-latitude")    -- récup. LATITUDE, LONGITUDE de station de base                            
  CIBLE_lon     = getValue("CIBLE-longitude")
    
  return CIBLE_lat, CIBLE_lon  

end --end function get_gps_CIBLE_info()

local function get_gps_uav_info()
  
  local UAV_Lat = 0
  local UAV_Lon = 0
  local UAV_alt = 0
  
    UAV_Lat      = getValue("latitude")          -- récup. LATITUDE, LONGITUDE, ALTITUDE du modèle
    UAV_Lon      = getValue("longitude")  
    UAV_alt      = getValue("gps-altitude")
  
  return UAV_Lat, UAV_Lon, UAV_alt  

end --end function get_gps_uav_info()

local function calc_pan_tilt(lon1, lat1, lon2, lat2, alt)
  
  local R = 6371000.0    --rayon de la terre 6371km
  local a, tc1, c, dist, dLat, dLon, azimut, elev
  dist = 0
  
  lon1=lon1/180*math.pi
  lat1=lat1/180*math.pi
  lon2=lon2/180*math.pi
  lat2=lat2/180*math.pi
  
  a = math.atan2(math.sin(lon2-lon1)*math.cos(lat2), math.cos(lat1)*math.sin(lat2)-math.sin(lat1)*math.cos(lat2)*math.cos(lon2-lon1))
  azimut  = a*180/math.pi
  azimut = azimut + 180
  if (azimut  < 0) then
    azimut = math.floor(360 + azimut)
    azimut = azimut + 180   
  end
 
-- Calcul de la distance entre l'UAV et la cible (cible)
  dLat = (lat2-lat1)
  dLon = (lon2-lon1)
  a = math.sin(dLat/2) * math.sin(dLat/2) + math.sin(dLon/2) * math.sin(dLon/2) * math.cos(lat1) * math.cos(lat2)
  c = 2* math.asin(math.sqrt(a))
  dist = 6371000 * c
  elev = math.atan(alt/dist)
  elev = math.floor(elev*360/(2*math.pi))
  elev = elev * -1
 
  return azimut , elev, dist

end --end function calc_pan_tilt()

local function azimut_elev_to_ppm(azimut, elev)
  
  local azimut_ppm_value = 0
  local elev_ppm_value   = 0
  
  if (azimut >= 270 and azimut <= 360) or (azimut >= 0 and azimut <= 90) then
    if (azimut >= 1 and azimut <= 90) then
      azimut_ppm_value = math.floor(map2(azimut, 0, 90, 0, 1024))
    else
      azimut_ppm_value = math.floor(map2(azimut, 270, 360, -1024, 0))
    end
    elev_ppm_value = math.floor(map2(elev, 0, 90, -1024, 0))
  else
    if (azimut >= 90 and azimut <= 180) then
      azimut_ppm_value = math.floor(map2(azimut, 90, 180, -1024, 0))
    else
      azimut_ppm_value = math.floor(map2(azimut, 180, 270, 0, 1024))
    end
    elev_ppm_value = math.floor(map2(elev, 90, 0, 0, 1024))
  end
  
  return azimut_ppm_value, elev_ppm_value

end -- end azimut_elev_to_ppm()

local function run()
  
  local CIBLE_lat          = 0
  local CIBLE_lon          = 0
  local UAV_Lat           = 0
  local UAV_Lon           = 0
  local UAV_alt           = 0
  local AZIMUT            = 0
  local ELEV              = 0
  local DIST              = 0
  local AZIMUT_PPM_VALUE  = 0
  local ELEV_PPM_VALUE    = -1024
  
  CIBLE_lat, CIBLE_lon = get_gps_CIBLE_info()
  
  if (CIBLE_lat ~= 0) and (CIBLE_lon ~= 0) then
    
    UAV_Lat, UAV_Lon, UAV_alt = get_gps_uav_info()
    
    if UAV_alt < 0 then
      
      UAV_alt = 0
      
    end
    
    AZIMUT, ELEV, DIST = calc_pan_tilt(CIBLE_lon, CIBLE_lat, UAV_Lon, UAV_Lat, UAV_alt)
    
    AZIMUT_PPM_VALUE, ELEV_PPM_VALUE = azimut_elev_to_ppm(AZIMUT, ELEV)
  else
    AZIMUT_PPM_VALUE = 0
    ELEV_PPM_VALUE   = -1024
  end
  
  return AZIMUT_PPM_VALUE, ELEV_PPM_VALUE
  
end --end function run()

return { run=run, output={ "PAN", "TILT"} }