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

Adding new follow target navigation and main states. New follow target

Browse Source 
topic added. New follow fsm added to the navigator
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent fc061ea94d
commit bbc8eaefd7
14 changed files with 266 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+7 -1
View File
@@ -187,7 +187,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
src/modules/mavlink/mavlink_receiver.cpp
+27 -2
View File
@@ -227,7 +227,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
case MAVLINK_MSG_ID_FOLLOW_TARGET:
handle_message_follow_target(msg);
break;
default:
break;
}
@@ -1623,6 +1625,30 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) {
mavlink_follow_target_t follow_me_msg;
mavlink_msg_follow_target_decode(msg, &follow_me_msg);
follow_target_s follow_target_topic = { };
follow_target_topic.timestamp = hrt_absolute_time();
memcpy(follow_target_topic.accel, follow_me_msg.acc, sizeof(follow_target_topic.accel));
memcpy(follow_target_topic.velocity, follow_me_msg.acc, sizeof(follow_target_topic.velocity));
//memcpy(follow_target_topic.attitude_q, follow_me_msg.attitude quaternion, sizeof(follow_target_topic.attitude_q));
follow_target_topic.lat = follow_me_msg.lat;
follow_target_topic.lon = follow_me_msg.lon;
if (_follow_me_pub == nullptr) {
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
} else {
warnx("publishing follow");
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
}
warnx("got message follow");
}
void
MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{
@@ -1804,7 +1830,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
}
}
/**
* Receive data from UART.
*/
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_force_setpoint.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include "mavlink_ftp.h"
@@ -136,6 +137,7 @@ private:
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_follow_target(mavlink_message_t *msg);
void *receive_thread(void *arg);
@@ -197,6 +199,7 @@ private:
orb_advert_t _manual_pub;
orb_advert_t _land_detector_pub;
orb_advert_t _time_offset_pub;
orb_advert_t _follow_me_pub;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;