Change quad-plane altitude in lua script while in automode


using a pixhawk
using ardupilot
using mission planner
programming language is lua script
rangefinder is LW20

Me and my team have a quad plane that we’re using for surveying I’ve been tasked with implementing a simple collision avoidance system to prevent the drone from crashing. The drone has a range finder pointing to the front of the vehicle to detect obstacles. The avoidance process is as follows.

Drone is performing normal surveying process in auto

step 1) constantly sample the rangefinder checking if it detects an object/cliff Infront of it within a certain threshold (80m). Sampling rate is every 100ms.

step 2) If an object/cliff is within this threshold then set flight mode to QLoiter. (I’ve been told by my team that this will stop the drone).

step 3) Once in QLoiter (and stationary infront of the object/cliff) increase altitude by 30m.

step 4) When altitude has been increased by 30m then check if the object/cliff is still there. If it is then repeat step 3 and 4. If it is not infront of drone then return to auto mode.

I am able to switch from auto to Qloiter and I am able to get the vehicles current altitude but I can’t get the altitude of the vehicle to change in lua script. I want to know of a way to do this while in Qloiter, or alternatively get a suggestion on how to perform what Im seeking better.

Below is the pseudo-code

local address = 102
local i2c_bus = i2c:get_device(0,0)

function update()


  returnTable = i2c_bus:read_registers(0, 2)      -- Read raw value
  distance = returnTable[1]*256 + returnTable[2]  -- Convert value to distance in cm

  gcs:send_text(0, "Distance ****" .. distance)

  if(distance < 5000) then                        -- If the distance is less than a threshold (50m) perform the hard stop
    vehicle:set_mode(19)                          -- Set vehicle to QLoiter, this instructs the vehicle to go into quad mode emulating a stop
    local position = ahrs:get_position()
    local altitude = position:alt()

    safeAlt = altitude + 3000                     -- Take the current altitude, in m, and add 30m
    set_alt(safeAlt)                              -- Set the vehicle to this safe altitude
    while(position:alt() < safeAlt) do            -- Constantly check current alt, halt this thread until altitude reaches safe zone

    vehicle:set_mode(10)                          -- Set vehicle to Auto, this instructs the vehicle to continue the preset mission

  return update, 100                                  

return update() 

Thank you for any responses