Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: add support for SET_POSITION_TARGET_GLOBAL_INT 

Also supports SET_POSITION_TARGET_LOCAL_NED but for both messages only the position fields are consumed.  Support for velocity, acceleration and yaw fields are not included.
This commit is contained in:
Randy Mackay 2016-08-23 15:04:30 +09:00
parent 29e18dba5b
commit 1181acc4e4
3 changed files with 93 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
APMrover2/GCS_Mavlink.cpp
View File

@ -1250,6 +1250,88 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
// prepare and send target position
if (!pos_ignore) {
Location loc = rover.current_loc;
if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
// rotate from body-frame to NE frame
float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
// add offset to current location
location_offset(loc, ne_x, ne_y);
} else if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED) {
// add offset to current location
location_offset(loc, packet.x, packet.y);
} else {
// MAV_FRAME_LOCAL_NED interpret as an offset from home
loc = rover.ahrs.get_home();
location_offset(loc, packet.x, packet.y);
}
rover.guided_WP = loc;
rover.rtl_complete = false;
rover.set_guided_WP();
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
// prepare and send target position
if (!pos_ignore) {
Location loc = rover.current_loc;
loc.lat = packet.lat_int;
loc.lng = packet.lon_int;
rover.guided_WP = loc;
rover.rtl_complete = false;
rover.set_guided_WP();
}
break;
}
case MAVLINK_MSG_ID_GPS_INPUT:
{
rover.gps.handle_msg(msg);

4
APMrover2/capabilities.cpp
View File

@ -6,5 +6,7 @@ void Rover::init_capabilities(void)
{
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
MAV_PROTOCOL_CAPABILITY_MISSION_INT);
MAV_PROTOCOL_CAPABILITY_MISSION_INT |
MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
}

8
APMrover2/defines.h
View File

@ -68,3 +68,11 @@ enum mode {
#define MASK_LOG_RC (1<<14)
#define MASK_LOG_ARM_DISARM (1<<15)
#define MASK_LOG_IMU_RAW (1UL<<19)
// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2))
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5))
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8))
#define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<9)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)