tarling - Path planning (blue line) is erratic and drone moves to wrong locations

DronAlan

Cliff Wong

@DronAlan Hi there, when the camera was rotated 180, you'll have stable take off behavior, but any XY movement would cause the drone to 'fly away' quickly.

Taking a step back with your concerns on physical mounts:

checking camera orientation:
a. bring up the front camera in portal: it should be upright orientation, e.g. top of the camera image should be from the top of the drone itself ("tops are aligned").
Since you are using qvio, only 1 camera is supported. Ensure in /etc/modalai/vio_cams.conf has tracking_down disabled, It's a safeguard if qvio is accidentally using that camera (would would lead to a orientation problem).

As for IMU, the voxl2 unit should have the USB-C port facing the bottom/ground side (and WiFi dongle on the top side). Let's force reset the imu calibration. If you run on the drone voxl-calibrate-imu and follow the directions, will reset the imu.

Let's check these items. If they all pass and still presents the orientation problem, then it's worth moving to the SDK1.6.3 to take advantage of the advanced config installer.

DronAlan

@Cliff-Wong said in tarling - Path planning (blue line) is erratic and drone moves to wrong locations:

/etc/modalai/vio_cams.conf

Hi there, thanks for the detailed suggestions!

I have gone through all your steps to discard physical and configuration issues, but it seems I'll need to upgrade. Here is the checklist of what I tested:

Camera orientation: I checked the front camera in voxl-portal and it is completely upright (tops are aligned, ceiling is up, floor is down).
Physical mounts: The VOXL 2 unit is mounted correctly from the factory (USB-C port facing the ground, WiFi dongle on the top side).
IMU Calibration: I ran a full 6DOF voxl-calibrate-imu on a flat surface. It completed successfully, and voxl-inspect-imu shows perfect readings now (X and Y at ~0.00, and Z gravity at exactly -9.80).
VIO config: I checked /etc/modalai/vio_cams.conf. tracking_down is disabled/not present, and it is only using the front camera as expected. Here is my exact configuration:

{
"cams": [{
"enable": true,
"name": "tracking",
"pipe_for_preview": "tracking",
"pipe_for_tracking": "tracking",
"is_occluded_on_ground": false,
"imu": "imu_apps",
"cal_file": "opencv_tracking_intrinsics.yml"
}]
}

Since all physical and configuration checks passed perfectly, I am going to follow your advice and proceed to flash SDK 1.6.3 to take advantage of the advanced config installer and move to OpenVINS.

Thanks again for the help, I'll keep you posted on how the 1.6.3 update goes!

DronAlan

@Cliff-Wong

I have just updated to SDK 1.6.3 doing a complete wipe and applying the factory configuration, but the drone is still executing the trajectory in reverse/backwards. I really don't know what else to try at this point. Any ideas on what could be causing this inverted behavior despite the clean install?

DronAlan

@Cliff-Wong

I am experiencing a severe issue with trajectory execution in the SLAM/Mapper mode where the drone exhibits an inverted and cumulative 'mirror' behavior regarding position setpoints.

As shown in the attached image:

If I set a first waypoint directly in front of the drone (1st Point), the drone flies backward instead, covering the intended distance but in the exact opposite direction.
From that new incorrect position, if I command the drone to return backward by the same distance (2nd point, aiming to return to the original starting location), the drone does not return. Instead, it flies forward, doubling the distance in the opposite direction from the newly commanded point.

It appears as though the drone is accumulating a starting error or interpreting the coordinate frame in a completely mirrored way (FRD vs NED mismatch), where both the direction and the origin reference point are inverted. The dashed lines in the image represent the intended path, while the solid arrows represent the actual path flown. I am on SDK 1.6.3 with factory settings.

Cliff Wong

@DronAlan With 1.6.3 SDK upgrade, did you test against the figure 8/offboard mode? Also flying in position mode after drawing the trajectory, if you manually fly the path you created, does the drone follow correctly? If position mode manual flying is working fine and figure 8/offboard mode is working fine (a clockwise figure then we have a trajectory calculation problem on the voxl-mapper side (I'll check our code, but our QA currently show that was working properly).

Since you're using OpenVINS vio module now, there's an unlikely chance of a reset causing a 180 orientation change by design. Also make sure qvio is disabled. You can check if it's running via voxl-inspect-services and run systemctl disable voxl-qvio-server (+reboot).

DronAlan

@Cliff-Wong

{
"config_file_version": 1,
"en_localhost_mavlink_udp": false,
"localhost_udp_port_number": 14551,
"en_vio": true,
"vio_pipe": "qvio",
"secondary_vio_pipe": "ov",
"vfc_vio_pipe": "ov",
"en_reset_vio_if_initialized_inverted": true,
"vio_warmup_s": 3,
"send_odom_while_failed": true,
"horizon_cal_tolerance": 0.5,
"en_hitl": false,
"offboard_mode": "figure_eight",
"follow_tag_id": 0,
"figure_eight_move_home": true,
"robot_radius": 0.300000011920929,
"collision_sampling_dt": 0.1,
"max_lookahead_distance": 1,
"wps_move_home": true,
"wps_stride": 0,
"wps_timeout": 0,
"wps_damp": 1,
"wps_vfc_mission": true,
"wps_vfc_mission_loop": false,
"wps_vfc_mission_to_ramp": 25,
"wps_vfc_mission_to_kp": 0.10000000149011612,
"wps_vfc_mission_cruise_speed": 1,
"tracking_trigger_ch": 12,
"tracking_trigger_ch_thresh_temp": 1200,
"tracking_trigger_ch_thresh_new": 1800,
"en_tag_fixed_frame": false,
"fixed_frame_filter_len": 5,
"en_transform_mavlink_pos_setpoints_from_fixed_frame": false,
"en_voa": true,
"voa_upper_bound_m": -0.15000000596046448,
"voa_lower_bound_m": 0.15000000596046448,
"voa_voa_memory_s": 1,
"voa_max_pc_per_fusion": 100,
"voa_pie_max_dist_m": 20,
"voa_pie_min_dist_m": 0.25,
"voa_pie_under_trim_m": 1,
"voa_pie_threshold": 3,
"voa_send_rate_hz": 20,
"voa_pie_slices": 36,
"voa_pie_bin_depth_m": 0.15000000596046448,
"voa_inputs": [{
"enabled": true,
"type": "point_cloud",
"input_pipe": "dfs_point_cloud",
"frame": "stereo_l",
"max_depth": 8,
"min_depth": 0.300000011920929,
"cell_size": 0.079999998211860657,
"threshold": 4,
"x_fov_deg": 68,
"y_fov_deg": 56,
"conf_cutoff": 0
}, {
"enabled": true,
"type": "point_cloud",
"input_pipe": "stereo_front_pc",
"frame": "stereo_front_l",
"max_depth": 8,
"min_depth": 0.300000011920929,
"cell_size": 0.079999998211860657,
"threshold": 4,
"x_fov_deg": 68,
"y_fov_deg": 56,
"conf_cutoff": 0
}, {
"enabled": true,
"type": "point_cloud",
"input_pipe": "stereo_rear_pc",
"frame": "stereo_rear_l",
"max_depth": 8,
"min_depth": 0.300000011920929,
"cell_size": 0.079999998211860657,
"threshold": 4,
"x_fov_deg": 68,
"y_fov_deg": 56,
"conf_cutoff": 0
}, {
"enabled": true,
"type": "tof",
"input_pipe": "tof",
"frame": "tof",
"max_depth": 6,
"min_depth": 0.15000000596046448,
"cell_size": 0.079999998211860657,
"threshold": 3,
"x_fov_deg": 106.5,
"y_fov_deg": 85.0999984741211,
"conf_cutoff": 125
}, {
"enabled": true,
"type": "rangefinder",
"input_pipe": "rangefinders",
"frame": "body",
"max_depth": 8,
"min_depth": 0.300000011920929,
"cell_size": 0.079999998211860657,
"threshold": 4,
"x_fov_deg": 68,
"y_fov_deg": 56,
"conf_cutoff": 0
}]
}

this is my /etc/modalai/voxl-vision-hub.conf, what should i change????

Cliff Wong

Hi there, just to confirm: does flying manually in position mode work and follows the XYZ position in voxl-portal's VIO page. as well as follows figure 8 mode too? This will confirm the vio pose estimate is correct and more of a issue on mapper.

DronAlan

@Cliff-Wong Yes, I can confirm that flying manually in Position mode works correctly and accurately follows the XYZ coordinates shown in the voxl-portal VIO page. Additionally, the drone successfully performs the figure 8 trajectories in offboard mode as expected.

Cliff Wong

@DronAlan
Hi there, the configuration & extrinsics files look fine (just switch offboard mode from figure_eight back to trajectory mode. Since you confirmed figure 8 and position mode are working fine and reflect true ground truth in voxl portal, next is to dive a bit deep into the processes:

Just to confirm: are you using a loaded saved map? Orientation can be different in a loaded maps based on how it was saved. Going forward in debugging this I would create the map while flying, then test plan-to-point to ensure it is working properly.

Before take off, if you ssh onto the drone and run voxl-vision-hub --debug_offboard and rerun the mapping test (point 1m in front of the drone hovering), and you don't need to map the entire room, just get the general area.
In the ssh session, monitor the output from voxl-vision-hub.
Take off and fly forward and backward in position mode, with some slight yaw motions to generate a decent map in voxl portal (recall we're generating a map in flight then plan to fly to a point, not using a old-saved map).
Then switch into trajectory mode. In the ssh session, you should get a Received trajectory has duration message printed.
Goto voxl-portal and plan-a-point. Then execute go-to-point. In the ssh session you should get a Received insert command. printed out on the terminal. And it will show the drone's forward commands, i..e set points, to your planned point. I expect the commanding: XYZ values to increase in the positive X direction. Please post the output of the ssh session here.

If the drone moves in the backwards direction as you've been seeing but the commanding XYZs are moving forward, we have found our problem (PX4 issue such that I'll need your params file). If the drone moves forward with commanding XYZs are moving forward then it's how your older maps are being saved and we can go from there.