# Where is apriltag relocalization used in control error signal?

Source: https://forum.modalai.com/topic/375/where-is-apriltag-relocalization-used-in-control-error-signal
Category: General Questions (https://forum.modalai.com/category/2/general-questions)
Posted: 2021-08-03 16:24:45 UTC by PawelJ
Replies: 1 · Views: 730

## PawelJ · 2021-08-03 16:24:45 UTC

I am working on updating the positions of a custom path in vvpx4 so that it is updated to be offset by the current position prior to switching to offboard mode. At first I have been using `T_body_wrt_local` from the `vio_manager.c`, but I notice that this value does not get changed with april tag detections. Following the chain, I see...
--> the `tag_manager` calls `geometry` when we detect new tags and have the correct config values set [here](https://gitlab.com/voxl-public/modal-pipe-architecture/voxl-vision-px4/-/blob/master/src/tag_manager.c#L110)
--> we get our updated transform from the pose_filter, that updates the pointer to `filter_local_to_fixed` with what we calculted as `new_T_local_wrt_fixed`, which I believe is the new drone state I want to use, set [here](https://gitlab.com/voxl-public/modal-pipe-architecture/voxl-vision-px4/-/blob/master/src/geometry.c#L528)
--> global variables get updated  with `rc_pose_filter_fetch` where the pointer to `T_local_wrt_fixed` gets overwritten with the value we just pushed to the `filter_local_to_fixed`, [here](https://gitlab.com/voxl-public/modal-pipe-architecture/voxl-vision-px4/-/blob/master/src/geometry.c#L837)

Now when we get the drone state from the `vio_manager` it only calls `geometry_get_T_body_wrt_local`, and I do not see where the drones state is updated with the new april tag offset. This could just be a misunderstanding on my part in how the system should work. The way I expect it to work is that the drone initializes it's state to be relative to starting position as the origin. This sets the global frame and is the reference frame the pixhawk expects drone state and targets to be set in. If this were the case, I would expect this to shift the state of the drone when a tag is detected (probably with some filtering on the px4 side that I'm also missing, to avoid the dicsontinuous jump in position). However, it appears that the drone state is set with `geometry_get_T_body_wrt_local`, and not the new fixed frame set by the april tag. 

Can you please clarify:
1. Where does the april tag localization change the drone position being sent to the pixhawk to affect the error signal of `target-position`? I will need to use this position so I can correctly offset my path starting point
2. Does the px4 expect path in the same reference frame as vio state (ie position in world coordinates, velocities in body frame?)

## Reply by PawelJ · 2021-08-05 19:53:24 UTC

1. Where does the april tag localization change the drone position being sent to the pixhawk to affect the error signal of target-position? I will need to use this position so I can correctly offset my path starting point

The setpoints are are sent to `uart_mavlink.c` where they are transformed by 
 `geometry_transform_fixed_setpoints_to_local` where the april tag transformation matrix is used to convert from fixed to local frame, which is sent off to the pixhawk. If no april tags are used local_frame == fixed_frame

2. Does the px4 expect path in the same reference frame as vio state (ie position in world coordinates, velocities in body frame?)
- px4 expects setpoints in local frame, but offboard figure eight assumes they are set in fixed frame (see above).

Please correct me if I am wrong here.

Cheers