diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-07 13:00:45 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-07 13:00:45 +0200 |
commit | 2d26e913921ce80e509bd79462319d09d12c2171 (patch) | |
tree | 897d69caa948fcdb6e2fde3adbae31e7e2683164 | |
parent | 58adea94699aa132ee568b9fe61a48f98eb42c78 (diff) | |
download | px4-firmware-2d26e913921ce80e509bd79462319d09d12c2171.tar.gz px4-firmware-2d26e913921ce80e509bd79462319d09d12c2171.tar.bz2 px4-firmware-2d26e913921ce80e509bd79462319d09d12c2171.zip |
WIP, change uorb offboard control sp topic
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 30 | ||||
-rw-r--r-- | src/modules/navigator/offboard.cpp | 30 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 34 |
3 files changed, 55 insertions, 39 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b72bfa858..e1a74f599 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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)); } diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 27ce46a1d..71a14c029 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -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; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2741f0c07..688135533 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -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); |