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

Merge branch 'rc_timeout' into mpc_rc

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-13 11:06:49 +04:00
parent dfd9601b57 0b97dd2b77
commit 88149311ea
31 changed files with 858 additions and 696 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+38 -71
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),
@@ -222,12 +220,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
@@ -254,7 +250,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
if (_flow_pub <= 0) {
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
@@ -288,7 +284,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
if (_cmd_pub <= 0) {
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
@@ -313,7 +309,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (_vicon_position_pub <= 0) {
if (_vicon_position_pub < 0) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
@@ -374,7 +370,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub <= 0) {
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
@@ -402,7 +398,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub <= 0) {
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@@ -416,47 +412,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);
}
}
@@ -620,11 +589,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.differential_pressure_timestamp = timestamp;
/* publish combined sensor topic */
if (_sensors_pub > 0) {
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
if (_sensors_pub < 0) {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
} else {
_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
}
}
@@ -639,11 +608,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
@@ -695,11 +664,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
if (_gps_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
} else {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
}
}
@@ -753,11 +722,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
if (_attitude_pub < 0) {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
}
}
@@ -777,11 +746,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
}
}
@@ -821,11 +790,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
}
}
@@ -862,11 +831,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
}
@@ -890,8 +859,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
View File
@@ -120,7 +120,6 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
int _manual_sub;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub;