From 85eed22150e4a24098554992a6d77bce6f1ddf31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 16:27:02 +0200 Subject: offboard sp: correctly map acceleration/force in combined setpoint to setpoint triple Conflicts: src/modules/uORB/topics/position_setpoint_triplet.h --- src/modules/mavlink/mavlink_receiver.cpp | 39 +++++++++++++++------- .../uORB/topics/offboard_control_setpoint.h | 21 ++++++++++++ .../uORB/topics/position_setpoint_triplet.h | 5 +++ 3 files changed, 53 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 418b81ee1..da651e4e7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 4f45ac14f..47c89f708 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -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)); } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ec2131abd..3ea3f671e 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -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 */ }; /** -- cgit v1.2.3