From feab2000cbda670f51bf75d360803e5552dedc20 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 10 Oct 2024 19:31:59 +0100 Subject: [PATCH] camtrack: add settings and pid tuning tools - Intial tune of status rates and pid gains - Reduce image status rate to 20 Hz - Set response_target in set message interval to flight-stack default - Add debug info for image status / tracker_pos - onboard_controller: apply formatter - onboard_controller: adjust controller gains - Port basic PID controller from ArduPilot AC_PID - onboard_controller: replace PID controllers - onboard_controller: use normalised coords for tracking - onboard_controller: increase k_i for tracking controllers - onboard_controller: add optional live graphs for pid analysis - onboard_controller: close connection on exit - Add settings and defaults Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/__init__.py | 97 ++++-- .../mavproxy_camtrack/onboard_controller.py | 269 ++++++++++----- .../modules/mavproxy_camtrack/pid_basic.py | 317 ++++++++++++++++++ .../mavproxy_camtrack/tracker_image.py | 7 + 4 files changed, 569 insertions(+), 121 deletions(-) create mode 100644 MAVProxy/modules/mavproxy_camtrack/pid_basic.py diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py index 264a71e624..84e41f5f24 100644 --- a/MAVProxy/modules/mavproxy_camtrack/__init__.py +++ b/MAVProxy/modules/mavproxy_camtrack/__init__.py @@ -1,6 +1,22 @@ """ MAVProxy camera tracking module +Examples +--------- + +localhost simulation +rtsp_url = "rtsp://127.0.0.1:8554/camera" + +home wifi +rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" + +herelink wifi access point +rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + +SIYI A8 camera +rtsp_url = "rtsp://192.168.144.25:8554/main.264" + + Reference --------- @@ -36,6 +52,10 @@ class CamTrackModule(mp_module.MPModule): """A tool to control camera tracking""" + # Module defaults + DEFAULT_RTSP_URL = "rtsp://127.0.0.1:8554/camera" + DEFAULT_IMAGE_STATUS_RATE = 20.0 + def __init__(self, mpstate): super(CamTrackModule, self).__init__( mpstate, "camtrack", "camera tracking module" @@ -44,23 +64,22 @@ def __init__(self, mpstate): self.mpstate = mpstate # Settings - # TODO: provide args to set RTSP server location - # localhost simulation - rtsp_url = "rtsp://127.0.0.1:8554/camera" - - # home wifi - # rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" - - # herelink wifi access point - # rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + self.camtrack_settings = mp_settings.MPSettings( + [ + ("rtsp_url", str, CamTrackModule.DEFAULT_RTSP_URL), + ("image_status_rate", float, CamTrackModule.DEFAULT_IMAGE_STATUS_RATE), + ] + ) - # SIYI A8 camera - # rtsp_url = "rtsp://192.168.144.25:8554/main.264" + # Commands + self.add_command("camtrack", self.cmd_camtrack, "camera tracking") - self._camera_tracking_image_status_rate = 50 # Hz + self._camera_tracking_image_status_rate = ( + self.camtrack_settings.image_status_rate + ) # GUI - self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url) + self.camera_view = None # NOTE: unused except for status # mavlink messages @@ -79,32 +98,36 @@ def __init__(self, mpstate): self._do_request_camera_information = True self._do_request_camera_tracking_image_status = True - # commands - self.add_command("camtrack", self.cmd_camtrack, "camera tracking") - def cmd_camtrack(self, args): - """Control behaviour of commands""" + """Command parser""" if len(args) <= 0: print(self.usage()) return - - if args[0] == "status": + if args[0] == "set": + if len(args) < 3: + self.camtrack_settings.show_all() + return + self.camtrack_settings.command(args[1:]) + elif args[0] == "status": print(self.status()) - return - - if args[0] == "start": - print("start tracking") - return + elif args[0] == "view": + self.cmd_view() + else: + print(self.usage()) - if args[0] == "stop": - print("stop tracking") + def cmd_view(self): + """Open camera view""" + if self.camera_view is not None: + print("camtrack view already open") return - print(self.usage()) + self.camera_view = CameraView( + self.mpstate, "Camera Tracking", self.camtrack_settings.rtsp_url + ) def usage(self): """Show help on command line options.""" - return "Usage: camtrack " + return "Usage: camtrack " def status(self): """Return information about the camera tracking state""" @@ -272,7 +295,9 @@ def handle_camera_information(self, msg): def handle_camera_tracking_image_status(self, msg): # TODO: refactor to use enums in onboard_controller.py if ( - msg.tracking_status == mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE + self.camera_view is not None + and msg.tracking_status + == mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE and msg.tracking_mode == mavutil.mavlink.CAMERA_TRACKING_MODE_RECTANGLE and msg.target_data == mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS ): @@ -282,7 +307,8 @@ def handle_camera_tracking_image_status(self, msg): def check_events(self): """Check for events on the camera view""" - self.camera_view.check_events() + if self.camera_view is not None: + self.camera_view.check_events() # TODO: check which shutdown events are available in MPImage # tell mavproxy to unload the module if the GUI is closed @@ -328,7 +354,7 @@ def send_messages(self): self.send_set_message_interval_message( mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, interval_us, # requested interval in microseconds - response_target=1, # flight-stack default + response_target=0, # flight-stack default ) self._do_request_camera_tracking_image_status = False @@ -369,9 +395,9 @@ def send_request_message(self, message_id, p1=0): ) def send_set_message_interval_message( - self, message_id, interval_us, response_target=1 + self, message_id, interval_us, response_target=0 ): - self.master.mav.command_long_send( + msg = self.master.mav.command_long_encode( self.settings.target_system, # target_system self.settings.target_component, # target_component mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command @@ -384,6 +410,7 @@ def send_set_message_interval_message( 0, # param6 response_target, # param7 ) + self.master.mav.send(msg) def idle_task(self): """Idle tasks""" @@ -392,7 +419,9 @@ def idle_task(self): def unload(self): """Close the GUI and unload module""" - self.camera_view.close() + if self.camera_view is not None: + self.camera_view.close() + self.camera_view = None def init(mpstate): diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 0a13966c57..7ffa113352 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -60,10 +60,15 @@ from enum import Enum from pymavlink import mavutil +# TODO: remember to add to requirements.txt / setup.py from transforms3d import euler from transforms3d import quaternions -from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import live_graph + +from MAVProxy.modules.mavproxy_camtrack.pid_basic import AC_PID_Basic +from MAVProxy.modules.mavproxy_camtrack.pid_basic import AP_PIDInfo + gi.require_version("Gst", "1.0") from gi.repository import Gst @@ -124,11 +129,12 @@ class CameraTrackingTargetData(Enum): class OnboardController: - def __init__(self, device, sysid, cmpid, rtsp_url): + def __init__(self, device, sysid, cmpid, rtsp_url, enable_graphs=False): self._device = device self._sysid = sysid self._cmpid = cmpid self._rtsp_url = rtsp_url + self._enable_graphs = enable_graphs # mavlink connection self._connection = None @@ -140,6 +146,9 @@ def __init__(self, device, sysid, cmpid, rtsp_url): print(f"Onboard Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") + def __del__(self): + self._connection.close() + def _connect_to_mavlink(self): """ Establish a mavlink connection. @@ -228,7 +237,9 @@ def run(self): # Create and register controllers self._camera_controller = CameraTrackController(self._connection) - self._gimbal_controller = GimbalController(self._connection) + self._gimbal_controller = GimbalController( + self._connection, self._enable_graphs + ) self._controllers.append(self._camera_controller) self._controllers.append(self._gimbal_controller) @@ -697,6 +708,12 @@ def _send_camera_tracking_image_status(self): rec_bottom_x, rec_bottom_y, ) + # NOTE: leave for debugging + # print( + # f"image_status: x: {rec_top_x:.2f}, y: {rec_top_y:.2f}, " + # f"w: {(rec_bottom_x - rec_top_x):.2f}, " + # f"h: {(rec_bottom_y - rec_top_y):.2f}" + # ) self._connection.mav.send(msg) def _handle_camera_track_point(self, msg): @@ -804,9 +821,9 @@ class GimbalController: Gimbal controller for onboard camera tracking. """ - def __init__(self, connection): + def __init__(self, connection, enable_graphs=False): self._connection = connection - self._sysid = self._connection.source_system # system id matches vehicle + self._sysid = self._connection.source_system # system id matches vehicle self._cmpid = 1 # component id matches autopilot print(f"Gimbal Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") @@ -830,9 +847,57 @@ def __init__(self, connection): self._gimbal_orientation = None self._gimbal_failure_flags = None - # TODO: add options for PI controller gains - self._pitch_controller = PI_controller(Pgain=0.1, Igain=0.01, IMAX=0.1) - self._yaw_controller = PI_controller(Pgain=0.1, Igain=0.1, IMAX=1.0) + # TODO: add options for PID controller gains + + # Pitch controller for centring gimbal + self._pit_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.0, + initial_d=0.0, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Yaw controller for centring gimbal + self._yaw_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.0, + initial_d=0.0, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Gimbal pitch controller for tracking + self._pit_track_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.2, + initial_d=0.01, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Gimbal yaw controller for tracking + self._yaw_track_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.2, + initial_d=0.01, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Analysis + self._enable_graphs = enable_graphs + self._graph_pid_tune = None + self._graph_pid_pit = None + self._graph_pid_yaw = None # Start the control thread self._control_in_queue = queue.Queue() @@ -858,8 +923,10 @@ def reset(self): self._tracking = False self._center_x = 0.0 self._center_y = 0.0 - self._pitch_controller.reset_I() - self._yaw_controller.reset_I() + self._pit_track_controller.reset_I() + self._pit_track_controller.reset_filter() + self._yaw_track_controller.reset_I() + self._yaw_track_controller.reset_filter() def mavlink_packet(self, msg): self._control_in_queue.put(msg) @@ -889,6 +956,10 @@ def _control_task(self): When not tracking, return the gimbal to its neutral position. """ + if self._enable_graphs: + self._add_livegraphs() + + last_time_s = time.time() while True: # Record the start time of the loop start_time = time.time() @@ -905,54 +976,61 @@ def _control_task(self): with self._mavlink_lock: gimbal_orientation = self._gimbal_orientation + # Update dt for the controller I and D terms + time_now_s = time.time() + dt = time_now_s - last_time_s + last_time_s = time_now_s + if not tracking and gimbal_orientation is not None: # NOTE: to centre the gimbal when not tracking, we need to know # the neutral angles from the MNT1_NEUTRAL_x params, and also # the current mount orientation. _, ay, az = euler.quat2euler(gimbal_orientation) - self._pitch_controller.gain_mul = 4.0 - self._yaw_controller.gain_mul = 40.0 + pit_rate_rads = self._pit_controller.update_all(self._neutral_y, ay, dt) + yaw_rate_rads = self._yaw_controller.update_all(self._neutral_z, az, dt) - err_pitch = self._neutral_y - ay - pitch_rate_rads = self._pitch_controller.run(err_pitch) - - err_yaw = self._neutral_z - az - yaw_rate_rads = self._yaw_controller.run(err_yaw) + pit_pid_info = self._pit_controller.pid_info + yaw_pid_info = self._yaw_controller.pid_info self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), - pitch_rate_rads, + pit_rate_rads, yaw_rate_rads, ) + + if self._enable_graphs: + self._update_livegraphs(pit_pid_info, yaw_pid_info) + elif frame_width is not None and frame_height is not None: - tgt_x = 0.5 * frame_width - tgt_y = 0.5 * frame_height + # work with normalised screen [-1, 1] + tgt_x = 0.0 + tgt_y = 0.0 if math.isclose(act_x, 0.0) and math.isclose(act_y, 0.0): - err_x = 0.0 - err_y = 0.0 + act_x = 0.0 + act_y = 0.0 else: - err_x = act_x - tgt_x - err_y = -(act_y - tgt_y) - - self._pitch_controller.gain_mul = 1.0 - self._yaw_controller.gain_mul = 2.0 + act_x = (act_x / frame_width - 0.5) * -1.0 + act_y = act_y / frame_height - 0.5 - err_pitch = math.radians(err_y) - pitch_rate_rads = self._pitch_controller.run(err_pitch) + pit_rate_rads = self._pit_track_controller.update_all(tgt_y, act_y, dt) + yaw_rate_rads = self._yaw_track_controller.update_all(tgt_x, act_x, dt) - err_yaw = math.radians(err_x) - yaw_rate_rads = self._yaw_controller.run(err_yaw) + pit_pid_info = self._pit_track_controller.pid_info + yaw_pid_info = self._yaw_track_controller.pid_info self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), - pitch_rate_rads, + pit_rate_rads, yaw_rate_rads, ) + if self._enable_graphs: + self._update_livegraphs(pit_pid_info, yaw_pid_info) + # Update at 50Hz update_period = 0.02 elapsed_time = time.time() - start_time @@ -985,47 +1063,61 @@ def __process_message(): sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) + def _add_livegraphs(self): + self._graph_pid_tune = live_graph.LiveGraph( + ["PID.PitTgt", "PID.PitAct", "PID.YawTgt", "PID.YawAct"], + timespan=30, + title="PID_TUNING", + ) + self._graph_pid_pit = live_graph.LiveGraph( + [ + "PID.Out", + "PID.P", + "PID.I", + "PID.D", + ], + timespan=30, + title="PID_TUNING: Pitch", + ) + self._graph_pid_yaw = live_graph.LiveGraph( + [ + "PID.Out", + "PID.P", + "PID.I", + "PID.D", + ], + timespan=30, + title="PID_TUNING: Yaw", + ) -class PI_controller: - """ - Simple PI controller - - MAVProxy/modules/mavproxy_SIYI/PI_controller (modified) - """ - - def __init__(self, Pgain, Igain, IMAX, gain_mul=1.0, max_rate=math.radians(30.0)): - self.Pgain = Pgain - self.Igain = Igain - self.IMAX = IMAX - self.gain_mul = gain_mul - self.max_rate = max_rate - self.I = 0.0 - - self.last_t = time.time() - - def run(self, err, ff_rate=0.0): - now = time.time() - dt = now - self.last_t - if now - self.last_t > 1.0: - self.reset_I() - dt = 0 - self.last_t = now - P = self.Pgain * self.gain_mul - I = self.Igain * self.gain_mul - IMAX = self.IMAX - max_rate = self.max_rate - - out = P * err - saturated = err > 0 and (out + self.I) >= max_rate - saturated |= err < 0 and (out + self.I) <= -max_rate - if not saturated: - self.I += I * err * dt - self.I = mp_util.constrain(self.I, -IMAX, IMAX) - ret = out + self.I + ff_rate - return mp_util.constrain(ret, -max_rate, max_rate) - - def reset_I(self): - self.I = 0 + def _update_livegraphs(self, pit_pid_info, yaw_pid_info): + if self._graph_pid_tune.is_alive(): + self._graph_pid_tune.add_values( + [ + pit_pid_info.target, + pit_pid_info.actual, + yaw_pid_info.target, + yaw_pid_info.actual, + ] + ) + if self._graph_pid_pit.is_alive(): + self._graph_pid_pit.add_values( + [ + pit_pid_info.out, + pit_pid_info.P, + pit_pid_info.I, + pit_pid_info.D, + ] + ) + if self._graph_pid_yaw.is_alive(): + self._graph_pid_yaw.add_values( + [ + yaw_pid_info.out, + yaw_pid_info.P, + yaw_pid_info.I, + yaw_pid_info.D, + ] + ) class TrackerCSTR: @@ -1073,34 +1165,37 @@ def set_normalised_roi(self, nroi): if __name__ == "__main__": import os import sys - from optparse import OptionParser + from argparse import ArgumentParser - parser = OptionParser("onboard_controller.py [options]") - parser.add_option("--master", default=None, type=str, help="MAVLink device") - parser.add_option("--rtsp-server", default=None, type=str, help="RTSP server URL") - parser.add_option("--sysid", default=1, type=int, help="Source system ID") - parser.add_option( + parser = ArgumentParser("onboard_controller.py [options]") + parser.add_argument("--master", required=True, type=str, help="MAVLink device") + parser.add_argument( + "--rtsp-server", required=True, type=str, help="RTSP server URL" + ) + parser.add_argument("--sysid", default=1, type=int, help="Source system ID") + parser.add_argument( "--cmpid", default=mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER, type=int, help="Source component ID", ) - - (opts, args) = parser.parse_args() - if opts.master is None: - print("Must specify a MAVLink device") - sys.exit(1) - if opts.rtsp_server is None: - print("Must specify an RTSP server URL") - sys.exit(1) + parser.add_argument( + "--enable-graphs", + action="store_const", + default=False, + const=True, + help="Enable live graphs", + ) + args = parser.parse_args() controller = OnboardController( - opts.master, opts.sysid, opts.cmpid, opts.rtsp_server + args.master, args.sysid, args.cmpid, args.rtsp_server, args.enable_graphs ) while True: try: controller.run() except KeyboardInterrupt: + controller = None EXIT_CODE_CTRL_C = 130 sys.exit(EXIT_CODE_CTRL_C) diff --git a/MAVProxy/modules/mavproxy_camtrack/pid_basic.py b/MAVProxy/modules/mavproxy_camtrack/pid_basic.py new file mode 100644 index 0000000000..accc694379 --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/pid_basic.py @@ -0,0 +1,317 @@ +""" +Python port of the ArduPilot basic PID controller. + +AC_PID_Basic.h +Generic PID algorithm, with EEPROM-backed storage of constants. +""" + +import math +import sys + + +# fftype.h: L63 +def is_zero(value): + return math.fabs(value) < sys.float_info.epsilon + + +# AP_Math.h: L75 +def is_negative(value): + return value <= (-1.0 * sys.float_info.epsilon) + + +# AP_Math.h: L64 +def is_positive(value): + return value >= sys.float_info.epsilon + + +# AP_Math.cpp: L266 +def constrain_value_line(amt, low, high): + # the check for NaN as a float prevents propagation of floating point + # errors through any function that uses constrain_value(). The normal + # float semantics already handle -Inf and +Inf + if amt is float("nan"): + raise Exception("constraining nan") + return (low + high) / 2.0 + + if amt < low: + return low + + if amt > high: + return high + + return amt + + +# AP_Math.h: L173 +def constrain_float(amt, low, high): + return constrain_value_line(float(amt), float(low), float(high)) + + +# AP_Math.cpp: L400 +# calculate a low pass filter alpha value +def calc_lowpass_alpha_dt(dt, cutoff_freq): + if is_negative(dt) or is_negative(cutoff_freq): + raise Exception("invalid_arg_or_result") + return 1.0 + if is_zero(cutoff_freq): + return 1.0 + if is_zero(dt): + return 0.0 + rc = 1.0 / (2.0 * math.pi * cutoff_freq) + return dt / (dt + rc) + + +class AP_PIDInfo: + """ + Data used in PID controllers + + This structure provides information on the internal member data of + a PID. It provides an abstract way to pass PID information around, + useful for logging and sending mavlink messages. + """ + + def __init__(self): + self.target = float(0.0) + self.actual = float(0.0) + self.error = float(0.0) + self.out = float(0.0) + self.P = float(0.0) + self.I = float(0.0) + self.D = float(0.0) + self.FF = float(0.0) + self.DFF = float(0.0) + self.Dmod = float(0.0) + self.slew_rate = float(0.0) + self.limit = bool(False) + self.PD_limit = bool(False) + self.reset = bool(False) + self.I_term_set = bool(False) + + +class AC_PID_Basic: + """ + AC_PID_Basic + + Copter PID control class + """ + + def __init__( + self, + initial_p, + initial_i, + initial_d, + initial_ff, + initial_imax, + initial_filt_E_hz, + initial_filt_D_hz, + ): + """ + Constructor for PID + """ + # parameters + self._kp = initial_p + self._ki = initial_i + self._kd = initial_d + self._kff = initial_ff + self._kimax = initial_imax + self._filt_E_hz = initial_filt_E_hz # PID error filter frequency in Hz + self._filt_D_hz = initial_filt_D_hz # PID derivative filter frequency in Hz + + # internal variables + self._target = 0.0 # target value to enable filtering + self._error = 0.0 # error value to enable filtering + self._derivative = 0.0 # last derivative for low-pass filter + self._integrator = 0.0 # integrator value + + # true when input filter should be reset during next call to set_input + self._reset_filter = True + + self._pid_info = AP_PIDInfo() + + # set target and measured inputs to PID controller and calculate outputs + # target and error are filtered + # the derivative is then calculated and filtered + # the integral is then updated based on the setting of the limit flag + def update_all(self, target, measurement, dt, limit=False): + return self.update_all_1( + target, + measurement, + dt, + (limit and is_negative(self._integrator)), + (limit and is_positive(self._integrator)), + ) + + def update_all_1(self, target, measurement, dt, limit_neg, limit_pos): + """ + update_all - set target and measured inputs to PID controller and calculate outputs + target and error are filtered + the derivative is then calculated and filtered + the integral is then updated based on the setting of the limit flag + """ + # don't process inf or NaN + if ( + not math.isfinite(target) + or math.isnan(target) + or not math.isfinite(measurement) + or math.isnan(measurement) + ): + raise Exception("invalid_arg_or_result") + return 0.0 + + self._target = target + + # reset input filter to value received + if self._reset_filter: + self._reset_filter = False + self._error = self._target - measurement + self._derivative = 0.0 + else: + error_last = self._error + self._error += self.get_filt_E_alpha(dt) * ( + (self._target - measurement) - self._error + ) + + # calculate and filter derivative + if is_positive(dt): + derivative = (self._error - error_last) / dt + self._derivative += self.get_filt_D_alpha(dt) * ( + derivative - self._derivative + ) + + # update I term + self.update_i(dt, limit_neg, limit_pos) + + P_out = self._error * self._kp + D_out = self._derivative * self._kd + + self._pid_info.target = self._target + self._pid_info.actual = measurement + self._pid_info.error = self._error + self._pid_info.P = self._error * self._kp + self._pid_info.I = self._integrator + self._pid_info.D = self._derivative * self._kd + self._pid_info.FF = self._target * self._kff + self._pid_info.out = P_out + self._integrator + D_out + self._target * self._kff + + return self._pid_info.out + + def update_i(self, dt, limit_neg, limit_pos): + """ + update the integral + if the limit flags are set the integral is only allowed to shrink + """ + if not is_zero(self._ki): + # Ensure that integrator can only be reduced if the output is saturated + if not ( + (limit_neg and is_negative(self._error)) + or (limit_pos and is_positive(self._error)) + ): + self._integrator += (self._error * self._ki) * dt + self._integrator = constrain_float( + self._integrator, -self._kimax, self._kimax + ) + + else: + self._integrator = 0.0 + + # get results from pid controller + def get_p(self): + return self._error * _kp + + def get_i(self): + return self._integrator + + def get_d(self): + return self._derivative * _kd + + def get_ff(self): + return self._target * _kff + + def get_error(self): + return self._error + + # reset the integrator + def reset_I(self): + self._integrator = 0.0 + + # input and D term filter will be reset to the next value provided to set_input() + def reset_filter(self): + self._reset_filter = True + + # get accessors + @property + def kP(self): + return self._kp + + @property + def kI(self): + return self._ki + + @property + def kD(self): + return self._kd + + @property + def ff(self): + return self._kff + + @property + def filt_E_hz(self): + return self._filt_E_hz + + @property + def filt_D_hz(self): + return self._filt_D_hz + + @property + def imax(self): + return self._kimax + + def get_filt_E_alpha(self, dt): + return calc_lowpass_alpha_dt(dt, self._filt_E_hz) + + def get_filt_D_alpha(self, dt): + return calc_lowpass_alpha_dt(dt, self._filt_D_hz) + + # set accessors + @kP.setter + def kP(self, value): + self._kp = value + + @kI.setter + def kI(self, value): + self._ki = value + + @kD.setter + def kD(self, value): + self._kd = value + + @ff.setter + def ff(self, value): + self._kff = value + + @imax.setter + def imax(self, value): + self._kimax = math.fabs(value) + + @filt_E_hz.setter + def filt_E_hz(self, hz): + self._filt_E_hz = math.fabs(hz) + + @filt_D_hz.setter + def filt_D_hz(self, hz): + self._filt_D_hz = math.fabs(hz) + + # integrator setting functions + def set_integrator_2(self, target, measurement, i): + self.set_integrator_1(target - measurement, i) + + def set_integrator_1(self, error, i): + self.set_integrator(i - error * self._kp) + + def set_integrator(self, i): + self._integrator = constrain_float(i, -self._kimax, self._kimax) + + @property + def pid_info(self): + return self._pid_info diff --git a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py index 84e0a5f8c9..42a221ecc5 100644 --- a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py +++ b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py @@ -279,6 +279,13 @@ def get_position(self): def set_position(self, tracker_pos): self._tracker_pos = tracker_pos + # NOTE: leave for debugging + # print( + # f"tracker_pos: x: {self._tracker_pos.left():.2f}, " + # f"y: {self._tracker_pos.top():.2f}, " + # f"w: {(self._tracker_pos.right() - self._tracker_pos.left()):.2f}, " + # f"h: {(self._tracker_pos.bottom() - self._tracker_pos.top()):.2f}" + # ) def create_tracker(name, state):