diff options
Diffstat (limited to 'apps/mavlink')
-rw-r--r-- | apps/mavlink/mavlink.c | 7 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 6 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 85 |
3 files changed, 59 insertions, 39 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 2ac803ce0..698e43f96 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -119,6 +119,7 @@ bool mavlink_hil_enabled = false; /* protocol interface */ static int uart; static int baudrate; +bool gcs_link = true; /* interface mode */ static enum { @@ -600,14 +601,14 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else { diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 15ed70dbd..550746794 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -93,6 +93,8 @@ static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; +extern bool gcs_link; + static void handle_message(mavlink_message_t *msg) { @@ -218,6 +220,10 @@ handle_message(mavlink_message_t *msg) //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); if (mavlink_system.sysid < 4) { + + /* switch to a receiving link mode */ + gcs_link = false; + /* * rate control mode - defined by MAVLink */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index c2428874f..0b073cc65 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -61,6 +61,8 @@ #include "mavlink_hil.h" #include "util.h" +extern bool gcs_link; + struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; @@ -172,15 +174,16 @@ l_sensor_combined(struct listener *l) baro_counter = raw.baro_counter; } - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1],raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); + if (gcs_link) + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + raw.gyro_rad_s[1], raw.gyro_rad_s[2], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1],raw.magnetometer_ga[2], + raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); sensors_raw_counter++; } @@ -194,8 +197,9 @@ l_vehicle_attitude(struct listener *l) /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, + if (gcs_link) + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, att.roll, att.pitch, @@ -271,8 +275,9 @@ l_rc_channels(struct listener *l) /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, + if (gcs_link) + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, @@ -322,7 +327,8 @@ l_local_position(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, @@ -344,7 +350,8 @@ l_global_position_setpoint(struct listener *l) if (global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, global_sp.lat, global_sp.lon, @@ -360,7 +367,8 @@ l_local_position_setpoint(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -376,7 +384,8 @@ l_attitude_setpoint(struct listener *l) /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, att_sp.timestamp/1000, att_sp.roll_body, att_sp.pitch_body, @@ -399,7 +408,8 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + if (gcs_link) + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number */, act_outputs.output[0], act_outputs.output[1], @@ -425,7 +435,8 @@ l_manual_control_setpoint(struct listener *l) /* copy manual control data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, + if (gcs_link) + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -441,23 +452,25 @@ l_vehicle_attitude_controls(struct listener *l) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators.control[3]); + } /* Only send in HIL mode */ if (mavlink_hil_enabled) { |