voxl_planner: send vx,vy,yaw_rate command
-
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