voxl_planner: send vx,vy,yaw_rate command

[Meytal Lempel]

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

[Meytal Lempel]

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