Hi!

I currently use voxl_planner to send position+yaw commands 1Hz:

void LocalOneStep::send_setpoint_position_to_position(Point3f &goal_pos, float goal_yaw)
{    
    setpoint_position_t msg;
    msg.magic_number = SETPOINT_POSITION_MAGIC_NUMBER;
    msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
    msg.type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE |
                    POSITION_TARGET_TYPEMASK_VY_IGNORE |
                    POSITION_TARGET_TYPEMASK_VZ_IGNORE |
                    POSITION_TARGET_TYPEMASK_AX_IGNORE |
                    POSITION_TARGET_TYPEMASK_AY_IGNORE |
                    POSITION_TARGET_TYPEMASK_AZ_IGNORE |
                    POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
    msg.position[0] = cos_north_wrt_fixed*goal_pos[0] - sin_north_wrt_fixed*goal_pos[1];
    msg.position[1] = sin_north_wrt_fixed*goal_pos[0] + cos_north_wrt_fixed*goal_pos[1];
    msg.position[2] = goal_z_m; //goal_pos[2];        
    msg.yaw = goal_yaw + north_wrt_fixed_rad;

    // to be ignored:
    msg.velocity[0] = 0.0;// [m/s]
    msg.velocity[1] = 0.0;// [m/s]
    msg.velocity[2] = 0.0; // [m/s]
    msg.yaw_rate = 0.0; // [rad/sec]
        
    

    pipe_server_write(plan_ch_, &msg, sizeof(setpoint_position_t));
}

Similarly, I would like to send commands vx,vy,z,yaw_rate :

void LocalOneStep::send_setpoint_position_velocity_and_yaw_rate(Point3f &cur_pos, float curr_yaw, Point3f &des_velocity, float des_yaw_rate)
{    
    setpoint_position_t msg;
    msg.magic_number = SETPOINT_POSITION_MAGIC_NUMBER;    
    msg.coordinate_frame = MAV_FRAME_LOCAL_NED; 
    // !! any other frame like MAV_FRAME_LOCAL_FRD is 
    // currently not supported by voxl-vision-hub
    msg.type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE |
                    POSITION_TARGET_TYPEMASK_Y_IGNORE |
                    POSITION_TARGET_TYPEMASK_VZ_IGNORE |
                    POSITION_TARGET_TYPEMASK_AX_IGNORE |
                    POSITION_TARGET_TYPEMASK_AY_IGNORE |
                    POSITION_TARGET_TYPEMASK_AZ_IGNORE |
                    POSITION_TARGET_TYPEMASK_YAW_IGNORE;

    msg.velocity[0] = cos_north_wrt_fixed*des_velocity[0] - sin_north_wrt_fixed*des_velocity[1];// [m/s]
    msg.velocity[1] = sin_north_wrt_fixed*des_velocity[0] + cos_north_wrt_fixed*des_velocity[1];// [m/s]
    msg.velocity[2] = 0.0; // [m/s] # to be ignored
    msg.yaw_rate = des_yaw_rate; // [rad/sec]

    // for stop in-place - see voxl-vision-hub offboard_trajetcory.c: _update_last_position
    msg.position[0] = cos_north_wrt_fixed*cur_pos[0] - sin_north_wrt_fixed*cur_pos[1];
    msg.position[1] = sin_north_wrt_fixed*cur_pos[0] + cos_north_wrt_fixed*cur_pos[1];
    msg.position[2] = goal_z_m; //cur_pos[2]; # to be included in cmd
    msg.yaw = curr_yaw + north_wrt_fixed_rad;    
    
    pipe_server_write(plan_ch_, &msg, sizeof(setpoint_position_t));
}

But It seems like my intentions are lost during the processing of voxl-vision-hub.

I use pymavlink to listen to the mavlink messages:

connection = mavutil.mavlink_connection('udpin:127.0.0.1:14551')
Below are my recordings of POSITION_TARGET_LOCAL_NED mavlink msg (@10Hz)

I would expect the setpoint values to remain constant until a new value is sent from voxl-planner (like it is with the x,y,z,yaw setpoint command)
Any thoughts?

Thank you

@shawn_ricardo Hi!

To my understanding EKF2_EV_CTRL 15-->13 only remove EV from pos_z estimation, so it should be ok to do that if the range sensor is active. However, I did revert all parameters to the default settings (matching the VOXL-SDK definitions).

As my attempts to integrate the range-sensor data in altitude estimation did not seem to succeed, I decided to try and integrate the range-sensor in the VIO solution.
I created a version of the voxl-qvio-server in which pos_z estimation from VIO is replaced by the equivalent value based on the rangefinder:

s.T_imu_wrt_vio[2] = - down_range_m;

This seem to work better, however it is a workaround and not a full solution. My current experiments are performed in low altitude (~40-90 cm), so I am not concerned with the limited range of the rangefinder sensor.

I'm not sure what is the current implemented behaviour when EV estimation is not good, there should be some fall-back relying on the range sensor and imu. From my short experience with such cases of poor EV estimation mid-flight, the drone just crushes or jump to the ceiling with no control.

Any suggestions or further explanations will be much appreciated. Thank you

@shawn_ricardo
Thank you for your response,

My GPS is not connected, and the px4 does get the distance sensor (I can see it through the qground app, matching the same values as in the right terminal "voxl-inspect-rangefinders")

Any other suggestions?

@Eric-Katzfey any thoughts?

@shawn_ricardo @Eric-Katzfey @Alex-Kushleyev
Hi!

We are flying indoors, and trying to configure PX4 for our vertical position to relay on the downward distance sensor only.

following https://docs.modalai.com/rangefinders/#px4-autopilot-height-estimate-integration :

EKF2_RNG_CTRL: (0-->1) set to 1 to enable integration of the rangefinder data into EKF2's height estimate
EKF2_HGT_REF: (3-->2) set to 2 to use the rangefinder as the primary height reference.

this was not enough, so in addition, we changed these:

EKF2_RNG_NOISE: 0.1-->0.01
EKF2_EV_CTRL 15-->13 (remove external vision sensor aiding from vertical position calc)
(EKF2_RNG_A_HMAX = 5)

In any case we couldn't achieve usage of downward distance sensor in altitude estimation :

Thank you in advance! Meytal and Valentin

@Eric-Katzfey Thank you! I now use journalctl and write to a file in a logger service, for example:

voxl-mapper-logger.service

[Unit]
Description=Logs voxl-mapper log output to file
After=voxl-wait-for-fs.service network.target network-online.target
Requires=voxl-wait-for-fs.service network.target network-online.target voxl-mapper.service

[Service]
Type=simple
ExecStart=/bin/bash -c "sleep 45 && journalctl -f -u voxl-mapper.service >> /data/logs/voxl_mapper.log"
StartLimitInterval=0

Restart=always
RestartSec=2s

[Install]
WantedBy=multi-user.target

@Eric-Katzfey thank you, but I want to download the textual notes that are written to a log file during the code execution of a service, not the recording of a pipe. Similar to dowload logs in the debug option in the voxl-portal. How do I do that?

Hi!
Where do I find the log files of each service (for example voxl-mapper) on voxl2?
(without using the voxl-portal)
Thank you

@Alex-Kushleyev Thank you, it works!

@Alex-Kushleyev Hi!

My team and I are working with both voxl1 and voxl2 based platforms. We used your example and changed it to publish a ros topic (Image) to pipe on voxl2. It works great. Now we try to do the same on voxl1 (compiling the same code to ipk) and encounter the problem:

Traceback (most recent call last):
  File "ros_img_mpa_pub.py", line 19, in <module>
    from pympa import *
  File "/usr/share/modalai/voxl-mpa-tools/pympa.py", line 23, in <module>
    pympa = ct.CDLL("libpympa.so")
  File "/usr/lib/python3.6/ctypes/__init__.py", line 348, in __init__
    self._handle = _dlopen(self._name, mode)
OSError: libpympa.so: wrong ELF class: ELFCLASS64

Is there any other branch that may be suitable for voxl1 to our purpose?

Thank you, Meytal