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

Merge branch 'navigator_new' into navigator_new_vector, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-28 10:04:13 +04:00
parent 153114aec8 72d9c80ce9
commit 7b7539fbbd
77 changed files with 5914 additions and 1412 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+17
View File
@@ -74,6 +74,8 @@
#include "waypoints.h"
#include "mavlink_parameters.h"
#include <uORB/topics/mission_result.h>
/* define MAVLink specific parameters */
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
@@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
thread_running = true;
/* arm counter to go off immediately */
@@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[])
lowspeed_counter++;
bool updated;
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
}
}
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
/* sleep quarter the time */
src/modules/mavlink/missionlib.c
+49 -40
View File
@@ -73,11 +73,15 @@
#include "waypoints.h"
#include "mavlink_parameters.h"
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
static uint64_t loiter_start_time;
#if 0
static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp);
#endif
int
mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -88,6 +92,8 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
return 0;
}
int
mavlink_missionlib_send_gcs_string(const char *string)
{
@@ -127,6 +133,7 @@ uint64_t mavlink_missionlib_get_system_timestamp()
return hrt_absolute_time();
}
#if 0
/**
* Set special vehicle setpoint fields based on current mission item.
*
@@ -206,7 +213,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
{
static orb_advert_t global_position_setpoint_pub = -1;
static orb_advert_t global_position_set_triplet_pub = -1;
// static orb_advert_t global_position_set_triplet_pub = -1;
static orb_advert_t local_position_setpoint_pub = -1;
static unsigned last_waypoint_index = -1;
char buf[50] = {0};
@@ -234,10 +241,10 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
/* fill triplet: previous, current, next waypoint */
struct vehicle_global_position_set_triplet_s triplet;
// struct vehicle_global_position_set_triplet_s triplet;
/* current waypoint is same as sp */
memcpy(&(triplet.current), &sp, sizeof(sp));
// memcpy(&(triplet.current), &sp, sizeof(sp));
/*
* Check if previous WP (in mission, not in execution order)
@@ -291,48 +298,48 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
/* populate last and next */
triplet.previous_valid = false;
triplet.next_valid = false;
// triplet.previous_valid = false;
// triplet.next_valid = false;
if (last_setpoint_valid) {
triplet.previous_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
wpm->waypoints[last_setpoint_index].param4,
wpm->waypoints[last_setpoint_index].command, &sp);
memcpy(&(triplet.previous), &sp, sizeof(sp));
}
// if (last_setpoint_valid) {
// triplet.previous_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[last_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[last_setpoint_index].param1,
// wpm->waypoints[last_setpoint_index].param2,
// wpm->waypoints[last_setpoint_index].param3,
// wpm->waypoints[last_setpoint_index].param4,
// wpm->waypoints[last_setpoint_index].command, &sp);
// memcpy(&(triplet.previous), &sp, sizeof(sp));
// }
if (next_setpoint_valid) {
triplet.next_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
wpm->waypoints[next_setpoint_index].param4,
wpm->waypoints[next_setpoint_index].command, &sp);
memcpy(&(triplet.next), &sp, sizeof(sp));
}
// if (next_setpoint_valid) {
// triplet.next_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[next_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[next_setpoint_index].param1,
// wpm->waypoints[next_setpoint_index].param2,
// wpm->waypoints[next_setpoint_index].param3,
// wpm->waypoints[next_setpoint_index].param4,
// wpm->waypoints[next_setpoint_index].command, &sp);
// memcpy(&(triplet.next), &sp, sizeof(sp));
// }
/* Initialize triplet publication if necessary */
if (global_position_set_triplet_pub < 0) {
global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
// if (global_position_set_triplet_pub < 0) {
// global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
} else {
orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
}
// } else {
// orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
// }
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
@@ -388,3 +395,5 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
printf("%s\n", buf);
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
}
#endif
src/modules/mavlink/orb_listener.c
+17 -16
View File
@@ -136,7 +136,7 @@ static const struct listener listeners[] = {
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
{l_global_position, &mavlink_subs.global_pos_sub, 0},
{l_local_position, &mavlink_subs.local_pos_sub, 0},
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
{l_global_position_setpoint, &mavlink_subs.triplet_sub, 0},
{l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
{l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
{l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
@@ -402,23 +402,24 @@ l_local_position(const struct listener *l)
void
l_global_position_setpoint(const struct listener *l)
{
struct vehicle_global_position_setpoint_s global_sp;
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
struct mission_item_triplet_s triplet;
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (!triplet.current_valid)
return;
if (global_sp.altitude_is_relative)
if (triplet.current.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
coordinate_frame,
global_sp.lat,
global_sp.lon,
global_sp.altitude * 1000.0f,
global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
(int32_t)(triplet.current.altitude * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
void
@@ -499,8 +500,8 @@ l_actuator_outputs(const struct listener *l)
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) {
/* only send in HIL mode and only send first group for HIL */
if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -656,7 +657,7 @@ l_home(const struct listener *l)
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
}
void
@@ -770,9 +771,9 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */
src/modules/mavlink/orb_topics.h
+3 -3
View File
@@ -50,9 +50,9 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -86,7 +86,7 @@ struct mavlink_subscriptions {
int local_pos_sub;
int spa_sub;
int spl_sub;
int spg_sub;
int triplet_sub;
int debug_key_value;
int input_rc_sub;
int optical_flow;
src/modules/mavlink/waypoints.c
+464 -325
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/waypoints.h
+20 -14
View File
@@ -56,6 +56,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/mission.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
@@ -81,7 +82,7 @@ enum MAVLINK_WPM_CODES {
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 15
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
#endif
@@ -92,33 +93,38 @@ enum MAVLINK_WPM_CODES {
struct mavlink_wpm_storage {
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
#endif
// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
// #endif
uint16_t size;
uint16_t max_size;
uint16_t rcv_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
// int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_firstinside_orbit;
uint64_t timestamp_lastoutside_orbit;
// uint64_t timestamp_last_send_setpoint;
// uint64_t timestamp_firstinside_orbit;
// uint64_t timestamp_lastoutside_orbit;
uint32_t timeout;
uint32_t delay_setpoint;
float accept_range_yaw;
float accept_range_distance;
bool yaw_reached;
bool pos_reached;
bool idle;
int current_dataman_id;
// uint32_t delay_setpoint;
// float accept_range_yaw;
// float accept_range_distance;
// bool yaw_reached;
// bool pos_reached;
// bool idle;
};
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void mavlink_wpm_init(mavlink_wpm_storage *state);
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);