ModalAI Forum
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Register
    • Login

    voxl_planner: send vx,vy,yaw_rate command

    VOXL SDK
    1
    1
    10
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • Meytal LempelM
      Meytal Lempel
      last edited by 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)

      Screenshot from 2025-07-02 12-53-20.png

      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

      1 Reply Last reply Reply Quote 0
      • First post
        Last post
      Powered by NodeBB | Contributors