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

WIP, change uorb offboard control sp topic

Browse Source
This commit is contained in:
Thomas Gubler
2014-07-07 13:00:45 +02:00
parent 58adea9469
commit 2d26e91392
3 changed files with 55 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+16 -14
View File
@@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
/* Convert values * 1000 back */
offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
//XXX: convert to quaternion
//offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
//offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
//offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
//offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
@@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
/* invalid setpoint, avoid publishing */
return;
}
offboard_control_sp.p1 = local_ned_position_setpoint_external.x;
offboard_control_sp.p2 = local_ned_position_setpoint_external.y;
offboard_control_sp.p3 = local_ned_position_setpoint_external.z;
offboard_control_sp.p4 = local_ned_position_setpoint_external.vx;
offboard_control_sp.p5 = local_ned_position_setpoint_external.vy;
offboard_control_sp.p6 = local_ned_position_setpoint_external.vz;
offboard_control_sp.p7 = local_ned_position_setpoint_external.afx;
offboard_control_sp.p8 = local_ned_position_setpoint_external.afy;
offboard_control_sp.p9 = local_ned_position_setpoint_external.afz;
offboard_control_sp.position[0] = local_ned_position_setpoint_external.x;
offboard_control_sp.position[1] = local_ned_position_setpoint_external.y;
offboard_control_sp.position[2] = local_ned_position_setpoint_external.z;
offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx;
offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy;
offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz;
offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx;
offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
for (int i = 0; i < 9; i++) {
offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i));
offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i));
offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i));
}
src/modules/navigator/offboard.cpp
+16 -14
View File
@@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
/* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not
* support deactivation of individual directions) */
if (_navigator->get_control_mode()->flag_control_position_enabled &&
(!_offboard_control_sp.ignore[0] &&
!_offboard_control_sp.ignore[1] &&
!_offboard_control_sp.ignore[2])) {
(!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))) {
/* position control */
pos_sp_triplet->current.x = _offboard_control_sp.p1;
pos_sp_triplet->current.y = _offboard_control_sp.p2;
pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
pos_sp_triplet->current.z = -_offboard_control_sp.p4;
pos_sp_triplet->current.x = _offboard_control_sp.position[0];
pos_sp_triplet->current.y = _offboard_control_sp.position[1];
//pos_sp_triplet->current.yaw = _offboard_control_sp.position[2];
//XXX: copy yaw
pos_sp_triplet->current.z = -_offboard_control_sp.position[2];
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
pos_sp_triplet->current.valid = true;
@@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
/* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not
* support deactivation of individual directions) */
if (_navigator->get_control_mode()->flag_control_velocity_enabled &&
(!_offboard_control_sp.ignore[3] &&
!_offboard_control_sp.ignore[4] &&
!_offboard_control_sp.ignore[5])) {
(!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))) {
/* velocity control */
pos_sp_triplet->current.vx = _offboard_control_sp.p2;
pos_sp_triplet->current.vy = _offboard_control_sp.p1;
pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
pos_sp_triplet->current.vz = _offboard_control_sp.p4;
pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0];
pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1];
// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[;
// //XXX: copy yaw speed
pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2];
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
pos_sp_triplet->current.valid = true;
src/modules/uORB/topics/offboard_control_setpoint.h
+23 -11
View File
@@ -81,19 +81,15 @@ struct offboard_control_setpoint_s {
enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */
double p1; /**< ailerons / roll / x pos / lat */
double p2; /**< elevator / pitch / y pos / lon */
float p3; /**< rudder / yaw / z pos / alt */
float p4; /**< throttle / x vel */
float p5; /**< roll rate / y vel */
float p6; /**< pitch rate / z vel */
float p7; /**< yaw rate / x acc */
float p8; /**< y acc */
float p9; /**< z acc */
double position[3]; /**< lat, lon, alt / x, y, z */
float velocity[3]; /**< x vel, y vel, z vel */
float acceleration[3]; /**< x acc, y acc, z acc */
float attitude[4]; /**< attitude of vehicle (quaternion) */
float attitude_rate[3]; /**< body angular rates (x, y, z) */
//XXX: use a bitmask with wrapper functions instead
bool ignore[9]; /**< if field i is set to true, pi should be ignored */
bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */
uint16_t ignore; /**< if field i is set to true, pi should be ignored */
bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
float override_mode_switch;
@@ -107,6 +103,22 @@ struct offboard_control_setpoint_s {
* @}
*/
inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << index));
}
inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
}
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)));
}
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)));
}
/* register this as object request broker structure */
ORB_DECLARE(offboard_control_setpoint);