Module/collective tracking control

Introduction

This module/algorithm allows a team of aircraft (they can fly at different speeds) to track a (possibly moving) target. In particular, the centroid of the team converges to the target as it is shown in the following screenshots

Custom messages

The module needs all-to-all communication (including the target). The following messages need to be included in your messages.xml.

These messages go in the telemetry section of messages.xml. The CTC_CONTROL message is just for telemetry porpuses.

    <message name="CTC" id="37">
      <field name="table" type="int16[]"/>
    </message>

    <message name="CTC_INFO_FROM_TARGET" id="40">
      <field name="px" type="float"/>
      <field name="py" type="float"/>
      <field name="vx" type="float"/>
      <field name="vy" type="float"/>
    </message>

    <message name="CTC_CONTROL" id="211">
        <field name="v_centroid_x" type="float" unit="m"/>
	<field name="v_centroid_y" type="float" unit="m"/>
	<field name="target_vx" type="float" unit="m/s"/>
	<field name="target_vy" type="float" unit="m/s"/>
	<field name="target_px" type="float" unit="m"/>
	<field name="target_py" type="float" unit="m"/>
	<field name="ref_px" type="float" unit="m"/>
	<field name="ref_py" type="float" unit="m"/>
    </message>

    <message name="CTC_INFO_TO_NEI" id="255">
        <field name="vx" type="float"/>
        <field name="vy" type="float"/>
        <field name="px" type="float"/>
        <field name="py" type="float"/>
    </message>

These messages go in the datalink section of messages.xml. They are necessary to init the algorithm from the ground control station.

    <message name="CTC_REG_TABLE" id="160" link="forwarded">
        <field name="ac_id" type="uint8"/>
        <field name="nei_id" type="uint8"/>
    </message>

    <message name="CTC_CLEAN_TABLE" id="161" link="forwarded">
        <field name="ac_id" type="uint8"/>
    </message>

Usage

Add the module in your flightplan

  <modules>
    <module name="collective_tracking_control">
    </module>
  </modules>

You need to define the neighboring relations of the aircraft. To do so, after you power up all the autopilots, you need to set an internal table by sending a series of commands with the help of the Python script ./PAPARAZZI_HOME/sw/ground_segment/python/collective_tracking_control/ctc_init.py . You must pass two arguments, the first one is an integer with the ID of the target, the second one a .txt file with the IDs of the rest of the aircraft.

The target will be broadcasting its position and velocity all the time. However, the rest of the aircraft will run the algorithm only if we tell them so in their corresponding navigation block, i.e.,

<block name="Collective tracking control">
  <call fun="collective_tracking_control()"/>
</block>

Tuning the gains, Python Simulation

The algorithm needs some parameters and gains to be set. The following Python simulation considers realistic orders of magnitude for distances and ground speeds. So you can try the gains and see the expected behavior of the algorithm before trying it in Paparazzi.

You can find the Python simulation at ./PAPARAZZI_HOME/sw/ground_segment/python/collective_tracking_control/ctc_simulation.py

WARNING

This algorithm commands roll angles for the aircraft and NO waypoints. If one aircraft stops receiving information from the other aircraft, then it will not update its roll angle and it might continue flying straight. Be sure to set safety measurements in your flight plan, for example:

<exceptions>
  <exception cond="dist2_to_home > (max_dist_from_home*max_dist_from_home)" deroute="STAND_BY"/>
</exceptions>