Moving Via PCMD

I am working in Olympe to create a way to control the drone. I have a function in python that gets the drone location from an outside sensor that takes a time x to run. During this time x, I want the drone to be moving via PCMD at a given speed. So far the only way that I have found to use PCMD is inside of a while loop where the PCMD command is sent every iteration of the loop at very fast intervals. I want to be able to have the drone move at a speed specified in PCMD for the duration of time x it takes my code to determine the drone position. Is there a way to send PCMD calls continuously while I am running other python code?
Very Simply:
assume old_coords and prev_loc are coordinates of the drone in x,y,z displacement in meters. Location_func gets the drone location in meters and it is stored in prev_loc. Fin is the final location in the z plane.
Right now PCMD is only being sent once a cycle of the while loop which is ~1 Hz. I want the PCMD to be sent at ~200Hz while the prev_loc[2] is less than the final location Fin.

def move_til_point(fin):
    old_coords = [0,0,0]
    prev_loc = [0,0,0]
    it = 0
    loop = 0;
    prev_loc, old_coords, it = location_func(prev_loc, old_coords, it)
    while prev_loc[2] <= fin and loop < 10:
        loop = loop + 1;
        start_time = time.time();
        drone(
                PCMD(1, 0, -10, 0, 0, int(time.time()))
         )
        prev_loc, old_coords, it = location_func(prev_loc, old_coords, it)
        print('previous location = ', prev_loc[2])
        
    print('All Done!')

Anyone have any idea how to do this?

Hi,

Olympe commands API (the drone(…) functor) is asynchronous so you don’t have to wait for the drone response. The drone(PCMD(1, 0, -10, 0, 0, int(time.time()))) should be pretty fast to execute.

Looking at your code, it seems like it is your location_func that takes 1 second to execute at each step of your control loop. In that case, one possible solution could be to run this function from another thread.

Something like :

class PCMDMoveTo(threading.Thread):
    def __init__(self, fin):
        self.fin = fin
        self.pcmd = None
        self.old_coords = [0,0,0]
        self.prev_loc = [0,0,0]
        self.it = 0
        self.loop = 0
        self.prev_loc, self.old_coords, self.it = location_func(self.prev_loc, self.old_coords, self.it)
        self.done = False

    def run(self):
        while not self.done:
             self.prev_loc, self.old_coords, self.it = location_func(self.prev_loc, self.old_coords, self.it)
             print('previous location = ', self.prev_loc[2])
             if self.prev_loc[2] > self.fin or self.loop >= 10:
                 self.done = True

    def step(self):
        if self.done:
           return True
        self.loop = self.loop + 1
        self.pcmd = drone(
            PCMD(1, 0, -10, 0, 0, 0))  # timestampAndSeqNum=0 is OK. Don't bother with this parameter

         )
        self.prev_loc, self.old_coords, self.it = location_func(self.prev_loc, self.old_coords, self.it)
        return False

def control_loop:
     fin = ...
     pcmd_move_to = PCMDMoveTo(fin)
     pcmd_move_to.start()
     while (some_condition):
         pcmd_move_to.step()
         # ... rest of the control loop ...

I haven’t tested this code but hope this help you get the general idea.

Regards
Nicolas

1 Like

I figured threading may be the solution to it, I’ll look into it! Thanks!