fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 08:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

offboard sp: correctly map acceleration/force in combined setpoint to setpoint triple

Browse Source 
Conflicts:
	src/modules/uORB/topics/position_setpoint_triplet.h
This commit is contained in:
Thomas Gubler
2014-08-12 16:27:02 +02:00
parent a2e6b2bca0
commit 85eed22150
3 changed files with 53 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+27 -12
View File
@@ -457,7 +457,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
}
} else {
/* It's not a force setpoint: publish to setpoint triplet topic */
/* It's not a pure force setpoint: publish to setpoint triplet topic */
struct position_setpoint_triplet_s pos_sp_triplet;
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
@@ -466,29 +466,44 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_position(offboard_control_sp, 0) &&
!offboard_control_sp_ignore_position(offboard_control_sp, 1) &&
!offboard_control_sp_ignore_position(offboard_control_sp, 2)) {
!offboard_control_sp_ignore_position_all(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = offboard_control_sp.position[0];
pos_sp_triplet.current.y = offboard_control_sp.position[1];
pos_sp_triplet.current.z = offboard_control_sp.position[2];
} else {
pos_sp_triplet.current.position_valid = false;
}
/* set the local vel values if the setpoint type is 'local pos' and none
* of the local vel fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_velocity(offboard_control_sp, 0) &&
!offboard_control_sp_ignore_velocity(offboard_control_sp, 1) &&
!offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
!offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
} else {
pos_sp_triplet.current.velocity_valid = false;
}
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
pos_sp_triplet.current.acceleration_is_force =
offboard_control_sp.isForceSetpoint;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
src/modules/uORB/topics/offboard_control_setpoint.h
+21
View File
@@ -142,14 +142,35 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro
return false;
}
/**
* Returns true if the acceleration setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
}
/**
* Returns true if all acceleration setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the bodyrate setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
}
/**
* Returns true if the attitude setpoint should be ignored
*/
inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
return (bool)(offboard_control_sp.ignore & (1 << 10));
}
src/modules/uORB/topics/position_setpoint_triplet.h
+5
View File
@@ -83,6 +83,11 @@ struct position_setpoint_s
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
float a_x; //**< acceleration x setpoint */
float a_y; //**< acceleration y setpoint */
float a_z; //**< acceleration z setpoint */
bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */
bool acceleration_is_force; //*< interprete acceleration as force */
};
/**