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

Merged mpc_rc into ekf_params

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-26 15:13:03 +02:00
parent a30411e9f2 84943644d7
commit 22d3bcdab6
40 changed files with 1026 additions and 943 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+6
View File
@@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
* If set to 1 incomming HIL GPS messages are parsed.
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
mavlink_system_t mavlink_system = {
100,
src/modules/mavlink/mavlink_main.cpp
+8
View File
@@ -217,6 +217,7 @@ Mavlink::Mavlink() :
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
@@ -496,11 +497,13 @@ void Mavlink::mavlink_update_system(void)
static param_t param_system_id;
static param_t param_component_id;
static param_t param_system_type;
static param_t param_use_hil_gps;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
param_use_hil_gps = param_find("MAV_USEHILGPS");
initialized = true;
}
@@ -525,6 +528,11 @@ void Mavlink::mavlink_update_system(void)
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
int32_t use_hil_gps;
param_get(param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
}
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
src/modules/mavlink/mavlink_main.h
+3
View File
@@ -157,6 +157,8 @@ public:
bool get_hil_enabled() { return _hil_enabled; }
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
@@ -223,6 +225,7 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
src/modules/mavlink/mavlink_receiver.cpp
+38 -52
View File
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
_manual_sub(-1),
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -162,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*
* Accept HIL GPS messages if use_hil_gps flag is true.
* This allows to provide fake gps measurements to the system.
*/
if (_mavlink->get_hil_enabled()) {
switch (msg->msgid) {
@@ -169,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_hil_sensor(msg);
break;
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
@@ -182,7 +179,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
}
}
/* If we've received a valid message, mark the flag indicating so.
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg);
break;
default:
break;
}
}
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
}
@@ -247,6 +257,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -417,47 +428,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
/* rc channels */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
rc.timestamp = hrt_absolute_time();
rc.chan_count = 4;
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
rc.chan[0].scaled = man.x / 1000.0f;
rc.chan[1].scaled = man.y / 1000.0f;
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
}
}
/* manual control */
{
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
}
@@ -768,7 +752,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -776,6 +759,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -787,19 +772,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* local position */
{
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
_hil_local_alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_lat = lat;
hil_local_pos.ref_lon = lon;
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@@ -887,8 +875,6 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -138,9 +138,9 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
float _hil_local_alt0;
struct map_projection_reference_s _hil_local_proj_ref;
};