Hi again,

From investigating the voxl-vision-hub code with added prints for debug, I see that the setpoint SETPOINT_POSITION_MAGIC_NUMBER is sent to mavlink-server from two functions -

(1) _send_current_position()
sending the current position, and type-mask ignoring vx,vy,vz,ax,ay,az,yaw_rate

// fetch latest position and attitude from px4 itself so the setpoint
// we are about to send is a close as possible to where we currently are

(2) execute_setpoint_position_command()
sending the setpoint as recieved from voxl-planner

The implemented behaviour of (1) suits the situation of position+yaw command. However for vx+vy+z+yaw_rate command I think something is missing.

Any help will be appreciated.
Thank you, Meytal

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!