forked from Archive/PX4-Autopilot
mavlink_receiver.cpp: fix coding style
This commit is contained in:
Beat Küng
Lorenz Meier
parent
5b684a77f4
commit
b427f5c90d
|
|
@ -273,7 +273,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
|
||||
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
|
||||
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);
|
||||
|
|
@ -285,7 +285,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
|
||||
}
|
||||
|
||||
/* If we've received a valid message, mark the flag indicating so.
|
||||
/* 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);
|
||||
}
|
||||
|
|
@ -295,17 +295,18 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c
|
|||
{
|
||||
/* evaluate if this system should accept this command */
|
||||
bool target_ok = false;
|
||||
|
||||
switch (command) {
|
||||
|
||||
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
|
||||
/* broadcast and ignore component */
|
||||
target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
|
||||
break;
|
||||
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
|
||||
/* broadcast and ignore component */
|
||||
target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
|
||||
break;
|
||||
|
||||
default:
|
||||
target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
|
||||
|| (target_component == MAV_COMP_ID_ALL));
|
||||
break;
|
||||
default:
|
||||
target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
|
||||
|| (target_component == MAV_COMP_ID_ALL));
|
||||
break;
|
||||
}
|
||||
|
||||
return target_ok;
|
||||
|
|
@ -330,6 +331,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||
|
||||
/* terminate other threads and this thread */
|
||||
_mavlink->_task_should_exit = true;
|
||||
|
||||
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
|
||||
/* send autopilot version message */
|
||||
_mavlink->send_autopilot_capabilites();
|
||||
|
|
@ -346,22 +348,35 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
struct vehicle_command_s vcmd;
|
||||
|
||||
memset(&vcmd, 0, sizeof(vcmd));
|
||||
|
||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = cmd_mavlink.param1;
|
||||
|
||||
vcmd.param2 = cmd_mavlink.param2;
|
||||
|
||||
vcmd.param3 = cmd_mavlink.param3;
|
||||
|
||||
vcmd.param4 = cmd_mavlink.param4;
|
||||
|
||||
vcmd.param5 = cmd_mavlink.param5;
|
||||
|
||||
vcmd.param6 = cmd_mavlink.param6;
|
||||
|
||||
vcmd.param7 = cmd_mavlink.param7;
|
||||
|
||||
// XXX do proper translation
|
||||
vcmd.command = cmd_mavlink.command;
|
||||
|
||||
vcmd.target_system = cmd_mavlink.target_system;
|
||||
|
||||
vcmd.target_component = cmd_mavlink.target_component;
|
||||
|
||||
vcmd.source_system = msg->sysid;
|
||||
|
||||
vcmd.source_component = msg->compid;
|
||||
|
||||
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||
|
||||
if (_cmd_pub == nullptr) {
|
||||
|
|
@ -410,22 +425,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
struct vehicle_command_s vcmd;
|
||||
|
||||
memset(&vcmd, 0, sizeof(vcmd));
|
||||
|
||||
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = cmd_mavlink.param1;
|
||||
|
||||
vcmd.param2 = cmd_mavlink.param2;
|
||||
|
||||
vcmd.param3 = cmd_mavlink.param3;
|
||||
|
||||
vcmd.param4 = cmd_mavlink.param4;
|
||||
|
||||
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
|
||||
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
|
||||
|
||||
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
|
||||
|
||||
vcmd.param7 = cmd_mavlink.z;
|
||||
|
||||
// XXX do proper translation
|
||||
vcmd.command = cmd_mavlink.command;
|
||||
|
||||
vcmd.target_system = cmd_mavlink.target_system;
|
||||
|
||||
vcmd.target_component = cmd_mavlink.target_component;
|
||||
|
||||
vcmd.source_system = msg->sysid;
|
||||
|
||||
vcmd.source_component = msg->compid;
|
||||
|
||||
if (_cmd_pub == nullptr) {
|
||||
|
|
@ -446,7 +473,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
|||
mavlink_msg_optical_flow_rad_decode(msg, &flow);
|
||||
|
||||
enum Rotation flow_rot;
|
||||
param_get(param_find("SENS_FLOW_ROT"),&flow_rot);
|
||||
param_get(param_find("SENS_FLOW_ROT"), &flow_rot);
|
||||
|
||||
struct optical_flow_s f;
|
||||
memset(&f, 0, sizeof(f));
|
||||
|
|
@ -477,10 +504,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
/* Use distance value for distance sensor topic */
|
||||
struct distance_sensor_s d;
|
||||
memset(&d, 0, sizeof(d));
|
||||
struct distance_sensor_s d = {};
|
||||
|
||||
if(flow.distance > 0.0f) { // negative values signal invalid data
|
||||
if (flow.distance > 0.0f) { // negative values signal invalid data
|
||||
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
|
||||
d.min_distance = 0.3f;
|
||||
d.max_distance = 5.0f;
|
||||
|
|
@ -492,7 +518,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
|||
|
||||
if (_flow_distance_sensor_pub == nullptr) {
|
||||
_flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d);
|
||||
}
|
||||
|
|
@ -544,7 +571,8 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
|||
|
||||
if (_hil_distance_sensor_pub == nullptr) {
|
||||
_hil_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(distance_sensor), _hil_distance_sensor_pub, &d);
|
||||
}
|
||||
|
|
@ -607,7 +635,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
|
|||
|
||||
if (_distance_sensor_pub == nullptr) {
|
||||
_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
&_orb_class_instance, ORB_PRIO_HIGH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
|
||||
|
|
@ -669,10 +697,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
|
||||
set_position_target_local_ned.target_system == 0) &&
|
||||
(mavlink_system.compid == set_position_target_local_ned.target_component ||
|
||||
set_position_target_local_ned.target_component == 0) &&
|
||||
values_finite) {
|
||||
set_position_target_local_ned.target_system == 0) &&
|
||||
(mavlink_system.compid == set_position_target_local_ned.target_component ||
|
||||
set_position_target_local_ned.target_component == 0) &&
|
||||
values_finite) {
|
||||
|
||||
/* convert mavlink type (local, NED) to uORB offboard control struct */
|
||||
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
|
||||
|
|
@ -704,24 +732,29 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
if (_mavlink->get_forward_externalsp()) {
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
if (is_force_sp && offboard_control_mode.ignore_position &&
|
||||
offboard_control_mode.ignore_velocity) {
|
||||
offboard_control_mode.ignore_velocity) {
|
||||
/* The offboard setpoint is a force setpoint only, directly writing to the force
|
||||
* setpoint topic and not publishing the setpoint triplet topic */
|
||||
struct vehicle_force_setpoint_s force_sp;
|
||||
force_sp.x = set_position_target_local_ned.afx;
|
||||
force_sp.y = set_position_target_local_ned.afy;
|
||||
force_sp.z = set_position_target_local_ned.afz;
|
||||
|
||||
//XXX: yaw
|
||||
if (_force_sp_pub == nullptr) {
|
||||
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* It's not a pure force setpoint: publish to setpoint triplet topic */
|
||||
struct position_setpoint_triplet_s pos_sp_triplet;
|
||||
|
|
@ -751,6 +784,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
pos_sp_triplet.current.x = set_position_target_local_ned.x;
|
||||
pos_sp_triplet.current.y = set_position_target_local_ned.y;
|
||||
pos_sp_triplet.current.z = set_position_target_local_ned.z;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.position_valid = false;
|
||||
}
|
||||
|
|
@ -761,6 +795,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
|
||||
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
|
||||
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.velocity_valid = false;
|
||||
}
|
||||
|
|
@ -773,7 +808,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
|
||||
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
|
||||
pos_sp_triplet.current.acceleration_is_force =
|
||||
is_force_sp;
|
||||
is_force_sp;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.acceleration_valid = false;
|
||||
|
|
@ -796,14 +831,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
} else {
|
||||
pos_sp_triplet.current.yawspeed_valid = false;
|
||||
}
|
||||
|
||||
//XXX handle global pos setpoints (different MAV frames)
|
||||
|
||||
if (_pos_sp_triplet_pub == nullptr) {
|
||||
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
|
||||
&pos_sp_triplet);
|
||||
&pos_sp_triplet);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
|
||||
&pos_sp_triplet);
|
||||
&pos_sp_triplet);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -837,10 +874,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||
PX4_ISFINITE(set_actuator_control_target.controls[7]);
|
||||
|
||||
if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
|
||||
set_actuator_control_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_actuator_control_target.target_component ||
|
||||
set_actuator_control_target.target_component == 0) &&
|
||||
values_finite) {
|
||||
set_actuator_control_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_actuator_control_target.target_component ||
|
||||
set_actuator_control_target.target_component == 0) &&
|
||||
values_finite) {
|
||||
|
||||
/* ignore all since we are setting raw actuators here */
|
||||
offboard_control_mode.ignore_thrust = true;
|
||||
|
|
@ -854,6 +891,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||
|
||||
if (_offboard_control_mode_pub == nullptr) {
|
||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
|
||||
}
|
||||
|
|
@ -862,6 +900,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||
/* If we are in offboard control mode, publish the actuator controls */
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
|
|
@ -871,12 +910,13 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
|
|||
actuator_controls.timestamp = hrt_absolute_time();
|
||||
|
||||
/* Set duty cycles for the servos in actuator_controls_0 */
|
||||
for(size_t i = 0; i < 8; i++) {
|
||||
for (size_t i = 0; i < 8; i++) {
|
||||
actuator_controls.control[i] = set_actuator_control_target.controls[i];
|
||||
}
|
||||
|
||||
if (_actuator_controls_pub == nullptr) {
|
||||
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
|
||||
}
|
||||
|
|
@ -942,10 +982,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_attitude_target.target_system ||
|
||||
set_attitude_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_attitude_target.target_component ||
|
||||
set_attitude_target.target_component == 0) &&
|
||||
values_finite) {
|
||||
set_attitude_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_attitude_target.target_component ||
|
||||
set_attitude_target.target_component == 0) &&
|
||||
values_finite) {
|
||||
|
||||
/* set correct ignore flags for thrust field: copy from mavlink message */
|
||||
_offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
|
||||
|
|
@ -969,6 +1009,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
|
||||
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
|
||||
_offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
|
||||
|
||||
} else {
|
||||
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
|
||||
_offboard_control_mode.ignore_attitude = ignore_attitude_msg;
|
||||
|
|
@ -992,6 +1033,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
if (_mavlink->get_forward_externalsp()) {
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
|
|
@ -1001,9 +1043,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
|
||||
if (!(_offboard_control_mode.ignore_attitude)) {
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (!ignore_attitude_msg) { // only copy att sp if message contained new data
|
||||
mavlink_quaternion_to_euler(set_attitude_target.q,
|
||||
&_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body);
|
||||
&_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body);
|
||||
mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body);
|
||||
_att_sp.R_valid = true;
|
||||
_att_sp.yaw_sp_move_rate = 0.0;
|
||||
|
|
@ -1017,6 +1060,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
|
||||
if (_att_sp_pub == nullptr) {
|
||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
|
||||
}
|
||||
|
|
@ -1026,17 +1070,20 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
|||
///XXX add support for ignoring individual axes
|
||||
if (!(_offboard_control_mode.ignore_bodyrate)) {
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
|
||||
_rates_sp.roll = set_attitude_target.body_roll_rate;
|
||||
_rates_sp.pitch = set_attitude_target.body_pitch_rate;
|
||||
_rates_sp.yaw = set_attitude_target.body_yaw_rate;
|
||||
}
|
||||
|
||||
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
|
||||
_rates_sp.thrust = set_attitude_target.thrust;
|
||||
}
|
||||
|
||||
if (_rates_sp_pub == nullptr) {
|
||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
|
||||
}
|
||||
|
|
@ -1105,6 +1152,7 @@ MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
|
|||
if (!on && (on != last_on)) {
|
||||
|
||||
_mom_switch_pos[sw]++;
|
||||
|
||||
if (_mom_switch_pos[sw] == state_count) {
|
||||
_mom_switch_pos[sw] = 0;
|
||||
}
|
||||
|
|
@ -1135,26 +1183,42 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
struct rc_input_values rc = {};
|
||||
|
||||
rc.timestamp_publication = hrt_absolute_time();
|
||||
|
||||
rc.timestamp_last_signal = rc.timestamp_publication;
|
||||
|
||||
rc.channel_count = 8;
|
||||
|
||||
rc.rc_failsafe = false;
|
||||
|
||||
rc.rc_lost = false;
|
||||
|
||||
rc.rc_lost_frame_count = 0;
|
||||
|
||||
rc.rc_total_frame_count = 1;
|
||||
|
||||
rc.rc_ppm_frame_length = 0;
|
||||
|
||||
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
|
||||
|
||||
rc.rssi = RC_INPUT_RSSI_MAX;
|
||||
|
||||
/* channels */
|
||||
rc.values[0] = man.chan1_raw;
|
||||
|
||||
rc.values[1] = man.chan2_raw;
|
||||
|
||||
rc.values[2] = man.chan3_raw;
|
||||
|
||||
rc.values[3] = man.chan4_raw;
|
||||
|
||||
rc.values[4] = man.chan5_raw;
|
||||
|
||||
rc.values[5] = man.chan6_raw;
|
||||
|
||||
rc.values[6] = man.chan7_raw;
|
||||
|
||||
rc.values[7] = man.chan8_raw;
|
||||
|
||||
if (_rc_pub == nullptr) {
|
||||
|
|
@ -1203,6 +1267,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||
/* decode all switches which fit into the channel mask */
|
||||
unsigned max_switch = (sizeof(man.buttons) * 8);
|
||||
unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
|
||||
|
||||
if (max_switch > (max_channels - 4)) {
|
||||
max_switch = (max_channels - 4);
|
||||
}
|
||||
|
|
@ -1211,6 +1276,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||
for (unsigned i = 0; i < max_switch; i++) {
|
||||
rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
|
||||
}
|
||||
|
||||
_mom_switch_state = man.buttons;
|
||||
|
||||
if (_rc_pub == nullptr) {
|
||||
|
|
@ -1272,9 +1338,10 @@ void
|
|||
MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_ping_t ping;
|
||||
mavlink_msg_ping_decode( msg, &ping);
|
||||
mavlink_msg_ping_decode(msg, &ping);
|
||||
|
||||
if ((mavlink_system.sysid == ping.target_system) &&
|
||||
(mavlink_system.compid == ping.target_component)) {
|
||||
(mavlink_system.compid == ping.target_component)) {
|
||||
mavlink_message_t msg_out;
|
||||
mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping);
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out);
|
||||
|
|
@ -1287,7 +1354,8 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
|
|||
mavlink_request_data_stream_t req;
|
||||
mavlink_msg_request_data_stream_decode(msg, &req);
|
||||
|
||||
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid && req.req_message_rate != 0) {
|
||||
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid
|
||||
&& req.req_message_rate != 0) {
|
||||
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
|
||||
|
||||
MavlinkStream *stream;
|
||||
|
|
@ -1315,11 +1383,12 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
|
|||
if (!onb_unix_valid && ofb_unix_valid) {
|
||||
tv.tv_sec = time.time_unix_usec / 1000000ULL;
|
||||
tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
|
||||
if(px4_clock_settime(CLOCK_REALTIME, &tv)) {
|
||||
|
||||
if (px4_clock_settime(CLOCK_REALTIME, &tv)) {
|
||||
warn("failed setting clock");
|
||||
}
|
||||
else {
|
||||
warnx("[timesync] UTC time synced.");
|
||||
|
||||
} else {
|
||||
warnx("[timesync] UTC time synced.");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1349,12 +1418,13 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
|
|||
|
||||
} else if (tsync.tc1 > 0) {
|
||||
|
||||
int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
|
||||
int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1 * 2) / 2 ;
|
||||
int64_t dt = _time_offset - offset_ns;
|
||||
|
||||
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
|
||||
_time_offset = offset_ns;
|
||||
warnx("[timesync] Hard setting offset.");
|
||||
|
||||
} else {
|
||||
smooth_time_offset(offset_ns);
|
||||
}
|
||||
|
|
@ -1382,17 +1452,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
|||
float dt;
|
||||
|
||||
if (_hil_last_frame == 0 ||
|
||||
(timestamp <= _hil_last_frame) ||
|
||||
(timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
|
||||
(_hil_last_frame >= timestamp)) {
|
||||
(timestamp <= _hil_last_frame) ||
|
||||
(timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
|
||||
(_hil_last_frame >= timestamp)) {
|
||||
dt = 0.01f; /* default to 100 Hz */
|
||||
memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro));
|
||||
_hil_prev_accel[0] = 0.0f;
|
||||
_hil_prev_accel[1] = 0.0f;
|
||||
_hil_prev_accel[2] = 9.866f;
|
||||
|
||||
} else {
|
||||
dt = (timestamp - _hil_last_frame) / 1e6f;
|
||||
}
|
||||
|
||||
_hil_last_frame = timestamp;
|
||||
|
||||
/* airspeed */
|
||||
|
|
@ -1645,12 +1717,13 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
|
|||
|
||||
follow_target_topic.timestamp = hrt_absolute_time();
|
||||
|
||||
follow_target_topic.lat = follow_target_msg.lat*1e-7;
|
||||
follow_target_topic.lon = follow_target_msg.lon*1e-7;
|
||||
follow_target_topic.lat = follow_target_msg.lat * 1e-7;
|
||||
follow_target_topic.lon = follow_target_msg.lon * 1e-7;
|
||||
follow_target_topic.alt = follow_target_msg.alt;
|
||||
|
||||
if (_follow_target_pub == nullptr) {
|
||||
_follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic);
|
||||
}
|
||||
|
|
@ -1666,8 +1739,8 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
|
|||
t.timestamp = hrt_absolute_time();
|
||||
|
||||
t.ICAO_address = adsb.ICAO_address;
|
||||
t.lat = adsb.lat*1e-7;
|
||||
t.lon = adsb.lon*1e-7;
|
||||
t.lat = adsb.lat * 1e-7;
|
||||
t.lon = adsb.lon * 1e-7;
|
||||
t.altitude_type = adsb.altitude_type;
|
||||
t.altitude = adsb.altitude / 1000.0f;
|
||||
t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F;
|
||||
|
|
@ -1683,6 +1756,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
|
|||
|
||||
if (_transponder_report_pub == nullptr) {
|
||||
_transponder_report_pub = orb_advertise(ORB_ID(transponder_report), &t);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
|
||||
}
|
||||
|
|
@ -1704,7 +1778,7 @@ void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg)
|
|||
if (pub == nullptr) {
|
||||
int idx = _gps_inject_data_next_idx;
|
||||
pub = orb_advertise_multi(ORB_ID(gps_inject_data), &gps_inject_data_topic,
|
||||
&idx, ORB_PRIO_DEFAULT);
|
||||
&idx, ORB_PRIO_DEFAULT);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
|
||||
|
|
@ -1840,7 +1914,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||
/* land detector */
|
||||
{
|
||||
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
|
||||
if(hil_land_detector.landed != landed) {
|
||||
|
||||
if (hil_land_detector.landed != landed) {
|
||||
hil_land_detector.landed = landed;
|
||||
hil_land_detector.timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
@ -1927,6 +2002,7 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
fds[0].fd = uart_fd;
|
||||
fds[0].events = POLLIN;
|
||||
}
|
||||
|
||||
#ifdef __PX4_POSIX
|
||||
struct sockaddr_in srcaddr = {};
|
||||
socklen_t addrlen = sizeof(srcaddr);
|
||||
|
|
@ -1960,31 +2036,38 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
usleep(sleeptime);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef __PX4_POSIX
|
||||
|
||||
if (_mavlink->get_protocol() == UDP) {
|
||||
if (fds[0].revents & POLLIN) {
|
||||
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
|
||||
}
|
||||
|
||||
} else {
|
||||
// could be TCP or other protocol
|
||||
}
|
||||
|
||||
struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
|
||||
struct sockaddr_in *srcaddr_last = _mavlink->get_client_source_address();
|
||||
|
||||
int localhost = (127 << 24) + 1;
|
||||
|
||||
if (!_mavlink->get_client_source_initialized()) {
|
||||
|
||||
// set the address either if localhost or if 3 seconds have passed
|
||||
// this ensures that a GCS running on localhost can get a hold of
|
||||
// the system within the first N seconds
|
||||
hrt_abstime stime = _mavlink->get_start_time();
|
||||
|
||||
if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000))
|
||||
|| (srcaddr_last->sin_addr.s_addr == htonl(localhost))) {
|
||||
|| (srcaddr_last->sin_addr.s_addr == htonl(localhost))) {
|
||||
srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr;
|
||||
srcaddr_last->sin_port = srcaddr.sin_port;
|
||||
_mavlink->set_client_source_initialized();
|
||||
warnx("changing partner IP to: %s", inet_ntoa(srcaddr.sin_addr));
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
// only start accepting messages once we're sure who we talk to
|
||||
|
||||
|
|
@ -2018,21 +2101,23 @@ void MavlinkReceiver::print_status()
|
|||
|
||||
uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
|
||||
{
|
||||
if(_time_offset > 0)
|
||||
if (_time_offset > 0) {
|
||||
return usec - (_time_offset / 1000) ;
|
||||
else
|
||||
|
||||
} else {
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavlinkReceiver::smooth_time_offset(int64_t offset_ns)
|
||||
{
|
||||
/* alpha = 0.6 fixed for now. The closer alpha is to 1.0,
|
||||
* the faster the moving average updates in response to
|
||||
* the faster the moving average updates in response to
|
||||
* new offset samples.
|
||||
*/
|
||||
|
||||
_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
|
||||
_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -2041,7 +2126,7 @@ void *MavlinkReceiver::start_helper(void *context)
|
|||
|
||||
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
|
||||
|
||||
void* ret = rcv->receive_thread(NULL);
|
||||
void *ret = rcv->receive_thread(nullptr);
|
||||
|
||||
delete rcv;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue