hi im trying to move a camera controlled by servos to the home position, the camera when start it allings with vehicule front, i am not sure how to get the angle respect the vehicule noose to point home, i had tryied this
–[[
send magnetic heading in degrees as NAMED_VALUE_FLOAT[MAG_HEAD] and NAMED_VALUE_FLOAT[MAG_GCRS]
–]]
local RATE_HZ = 2
– bind a parameter to a variable given
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format(‘could not find %s parameter’, name))
return p
end
COMPASS_DEC = bind_param(“COMPASS_DEC”)
function wrap_360(angle)
local res = angle % 360
if res < 0 then
res = res + 360
end
return res
end
function update()
local yaw_deg = wrap_360(math.deg(ahrs:get_yaw() - COMPASS_DEC:get()))
local gspd = ahrs:groundspeed_vector()
local gcrs_deg = wrap_360(math.deg(math.atan(gspd:y(), gspd:x()) - COMPASS_DEC:get()))
local position = ahrs:get_location()
local home= ahrs:get_home()
if home and position then
local bearing = wrap_360(math.deg(position:get_bearing(home)))
local target_bearing_deg = wrap_360(math.deg(position:get_bearing(home))+yaw_deg)
local target_bearing_deg2 = wrap_360(math.deg(position:get_bearing(home))+gcrs_deg)
gcs:send_named_float(“MAG_HEAD”, yaw_deg)
gcs:send_named_float(“MAG_GCRS”, gcrs_deg)
gcs:send_named_float(“Bearing”, bearing)
gcs:send_named_float(“MAG_HEAD_HOME”, target_bearing_deg)
gcs:send_named_float(“MAG_GCRS_HOME”, target_bearing_deg2)
end
end
gcs:send_text(0, “MagHeading loaded”)
local MAV_SEVERITY_ERROR = 3
– wrapper around update(). This calls update() and if update faults
– then an error is displayed, but the script is not stopped
function protected_wrapper()
local success, err = pcall(update)
if not success then
gcs:send_text(MAV_SEVERITY_ERROR, "Internal Error: " … err)
– when we fault we run the update function again after 1s, slowing it
– down a bit so we don’t flood the console with errors
return protected_wrapper, 1000
end
return protected_wrapper, math.floor(1000 / RATE_HZ)
end
– start running update loop
return protected_wrapper()
bearing its kinda works but its not taking in consideration the yaw of the vehicule