[SOLVED] Unresponsive logging sequence

Firmware/software/electronics/mechanics
Post Reply
pizzicato268
Beginner
Posts: 1
Joined: Thu Mar 07, 2019 3:38 pm

[SOLVED] Unresponsive logging sequence

Post by pizzicato268 »

Hi!

I'm an electrical engineering student doing my final year thesis on visual control systems. I have connected an onboard camera on the crazyflie and my goal is to have it yaw towards a specific target, go to it and land. So far, it seems to be working fine, but I have a problem with the logging API in Python. I'm using the synchronous logging scheme and for some reason, after some time (usually 15-30 sec) the logging stops and the whole system becomes unresponsive. Here's me running the program: https://www.youtube.com/watch?v=a90CCFLT794

Here's part of the main loop:

Code: Select all

with SyncCrazyflie(URI) as scf:
                # Define motion commander to crazyflie as mc:
                cf2 = CF(scf, available)
                cf2_psi = cf2.psi    # Get current yaw-angle of cf
                cf2_bat = cf2.vbat  # Get current battery level of cf
                marker_psi = 0  # Set marker angle to zero initially

                pid = PID(0, 0, 0, setpoint=marker_psi)  # Initialize PID
                # Define pid parameters
                pid.tunings = (Kp, Kd, Ki)
                pid.output_limits = (-60, 60)
                pid.proportional_on_measurment = False

                if cf2_bat >= low_battery:
                    cf2.takeoff()    # CF takes off from ground
                    print("Taking off ! Battery level: " + str(cf2_bat))
                    time.sleep(1.5)   # Let the CF hover for 1.5s

                t_init = time.time()
                endTime = t_init + 30   # Let the CF do its thing for some time

                # Add values to array for logging/plotting
                setpoint = np.append(setpoint, marker_psi)  # Commanded yaw
                yaw_cf = np.append(yaw_cf, cf2.psi)  # Actual yaw of CF
                ar_time = np.append(ar_time, 0)    # Time vector
                ar_bat = np.append(ar_bat, cf2_bat)
                ar_cf2_yaw_corr = np.append(ar_cf2_yaw_corr, marker_psi-yaw_cf)

                A = scf.is_link_open()
                while time.time() < endTime and cf2_bat >= low_battery and A:
                    # Get angle of marker
                    try:
                        # Get marker yaw-angle, img and translation vector(xyz)
                        marker_psi, img, t_vec = getMarkerAngles(cam)

                        # Get translational distance in Z-dir, marker->lens
                        if t_vec is not None:
                            try:
                                dist_z = np.asarray(t_vec).tolist()
                                dist_z = np.reshape(dist_z, [1, 3])
                                dist_z = dist_z[0, 2]
                            except ValueError:
                                print(err_dict["1"])
                        else:
                            dist_z = None

                        # Moving average filter
                        if marker_psi is not None:
                            zs = np.append(zs, marker_psi)
                            ds = np.append(ds, dist_z)
                            marker_psi = np.average(zs)
                            dist_z = np.average(ds)
                            if np.size(zs) >= samples_max:
                                zs = np.delete(zs, 0)
                                ds = np.delete(ds, 0)

                        # perform kalman filtering
                        f.update(marker_psi)    # yaw
                        f.predict()
                        d.update(dist_z)    # transl_z
                        d.predict()

                        # Set an inertial degregation to velocity for each iteration
                        if marker_psi is None and nf < nf_max and None not in (f.x[0], d.x[0], cf2_psi):
                            # f.x and d.x holds the estimated values
                            f.x[1] *= alpha
                            d.x[1] *= alpha
                            yaw_cmd = f.x[0, 0]  # commanded yaw
                            cf2_yaw_corr = -cf2_psi  #- f.x[0, 0]   # compensated yaw
                            transl_z_cmd = round(d.x[0, 0])    # commanded transl_z
                            nf += 1
                        elif marker_psi is not None and marker_psi != 0:
                            nf = 0
                            yaw_cmd = marker_psi  # commanded yaw
                            cf2_yaw_corr = -cf2_psi  #- marker_psi  # compensated yaw
                            transl_z_cmd = round(dist_z)   # commanded transl_z
                        else:
                            yaw_cmd = 0
                            print("Marker not found")

                    except (cv2.error, TypeError, AttributeError):
                        print(err_dict["3"])
                        print("Error: ", sys.exc_info()[0])
                        cam.release()
                        cv2.destroyAllWindows()
                        cf2.land()  # Land the CF2
                        print(TypeError)
                        traceback.print_exc()
                        break

                    pid.setpoint = yaw_cmd  # Assign setpoint to PID-ctrl
                    dt = time.time() - t_init  # compute elapsed time

                    # print text on image
                    cv2.putText(img, "t: "+str(round(dt, 2)),
                                timestampPosition,
                                font,
                                fontScale,
                                fontColor,
                                lineType)
                    cv2.putText(img, "psi: "+str(round(yaw_cmd, 2)),
                                yawstampPosition,
                                font,
                                fontScale,
                                fontColor,
                                lineType)

                    if dispLiveVid:
                        cv2.imshow('image', img)    # display current frame

                        if cv2.waitKey(25) & 0xFF == ord('q'):
                            cf2.land()
                            cv2.destroyAllWindows()
                            break

                    out.write(img)  # write img frame to video file
                    # Send command signal to CF2 and update cf2_psi
                    # 'psi' = yaw
                    # 'theta' = pitch
                    # 'phi' = roll
                    # 'z' = forwards/backwards
                    #  ** Send signal to CF + get battery level and yaw-angle
                    try:
                        cf2_psi, cf2_bat = cf2.update(pid.setpoint, 'psi', turnRate)
The last row: cf2_psi, cf2_bat = cf2.update(pid.setpoint, 'psi', turnRate), is where the process usually fail after some time.
The update function calls the synchronous logging:

Code: Select all

def getData(scf, available):
    lg_stab = LogConfig(name='Stabilizer', period_in_ms=12)
    # lg_stab.add_variable('stabilizer.roll', 'float')
    lg_stab.add_variable('pm.vbat', 'float')
    lg_stab.add_variable('stabilizer.yaw', 'float')

    with SyncLogger(scf, lg_stab) as logger:
        for log_entry in logger:
            # timestamp = log_entry[0]
            data = log_entry[1]
            # logconf_name = logger[2]
            y = data.get('stabilizer.yaw')
            vbat = data.get('pm.vbat')
            return(y, vbat)
I would really appreciate some input on this matter, as I've been stuck on this issue for some time. Would it work better if I used the basicLogging script instead of the synchronous one ? What would be the easiest way to implement it ? I'm quite new to Python :lol:

Thanks!

EDIT: I solved the issue, the problem was that I call the

Code: Select all

with SyncLogger(scf, lg_stab) as logger:
        for log_entry in logger:
every time I call the function. By simply putting this in the main loop instead, the issue went away :)
tobias
Bitcraze
Posts: 2339
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: [SOLVED] Unresponsive logging sequence

Post by tobias »

Great that you found the issue!
Post Reply