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

Accomodating for offboard control setups

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-22 16:08:48 +02:00
parent 64c5096c9f
commit ea36154e39
4 changed files with 61 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+4 -3
View File
@@ -119,6 +119,7 @@ bool mavlink_hil_enabled = false;
/* protocol interface */ /* protocol interface */
static int uart; static int uart;
static int baudrate; static int baudrate;
bool gcs_link = true;
/* interface mode */ /* interface mode */
static enum { 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); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) { } else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */ /* 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_RAW_IMU, 300);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */ /* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */ /* 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 */ /* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else { } else {
apps/mavlink/mavlink_receiver.c
+6
View File
@@ -93,6 +93,8 @@ static orb_advert_t flow_pub = -1;
static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1; static orb_advert_t vicon_position_pub = -1;
extern bool gcs_link;
static void static void
handle_message(mavlink_message_t *msg) 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); //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) { if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/* /*
* rate control mode - defined by MAVLink * rate control mode - defined by MAVLink
*/ */
apps/mavlink/orb_listener.c
+49 -36
View File
@@ -61,6 +61,8 @@
#include "mavlink_hil.h" #include "mavlink_hil.h"
#include "util.h" #include "util.h"
extern bool gcs_link;
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos; struct vehicle_local_position_s local_pos;
struct vehicle_status_s v_status; struct vehicle_status_s v_status;
@@ -172,15 +174,16 @@ l_sensor_combined(struct listener *l)
baro_counter = raw.baro_counter; baro_counter = raw.baro_counter;
} }
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, if (gcs_link)
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
raw.gyro_rad_s[1], raw.gyro_rad_s[2], raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
raw.magnetometer_ga[0], raw.gyro_rad_s[1], raw.gyro_rad_s[2],
raw.magnetometer_ga[1],raw.magnetometer_ga[2], raw.magnetometer_ga[0],
raw.baro_pres_mbar, 0 /* no diff pressure yet */, raw.magnetometer_ga[1],raw.magnetometer_ga[2],
raw.baro_alt_meter, raw.baro_temp_celcius, raw.baro_pres_mbar, 0 /* no diff pressure yet */,
fields_updated); raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
sensors_raw_counter++; sensors_raw_counter++;
} }
@@ -194,8 +197,9 @@ l_vehicle_attitude(struct listener *l)
/* copy attitude data into local buffer */ /* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
/* send sensor values */ if (gcs_link)
mavlink_msg_attitude_send(MAVLINK_COMM_0, /* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000, last_sensor_timestamp / 1000,
att.roll, att.roll,
att.pitch, att.pitch,
@@ -271,8 +275,9 @@ l_rc_channels(struct listener *l)
/* copy rc channels into local buffer */ /* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc); orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */ if (gcs_link)
mavlink_msg_rc_channels_raw_send(chan, /* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
rc.timestamp / 1000, rc.timestamp / 1000,
0, 0,
rc.chan[0].raw, rc.chan[0].raw,
@@ -322,7 +327,8 @@ l_local_position(struct listener *l)
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); 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.timestamp / 1000,
local_pos.x, local_pos.x,
local_pos.y, local_pos.y,
@@ -344,7 +350,8 @@ l_global_position_setpoint(struct listener *l)
if (global_sp.altitude_is_relative) if (global_sp.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; 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, coordinate_frame,
global_sp.lat, global_sp.lat,
global_sp.lon, global_sp.lon,
@@ -360,7 +367,8 @@ l_local_position_setpoint(struct listener *l)
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); 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, MAV_FRAME_LOCAL_NED,
local_sp.x, local_sp.x,
local_sp.y, local_sp.y,
@@ -376,7 +384,8 @@ l_attitude_setpoint(struct listener *l)
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); 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.timestamp/1000,
att_sp.roll_body, att_sp.roll_body,
att_sp.pitch_body, att_sp.pitch_body,
@@ -399,7 +408,8 @@ l_actuator_outputs(struct listener *l)
/* copy actuator data into local buffer */ /* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs); 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 */, l->arg /* port number */,
act_outputs.output[0], act_outputs.output[0],
act_outputs.output[1], act_outputs.output[1],
@@ -425,7 +435,8 @@ l_manual_control_setpoint(struct listener *l)
/* copy manual control data into local buffer */ /* copy manual control data into local buffer */
orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); 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, mavlink_system.sysid,
man_control.roll * 1000, man_control.roll * 1000,
man_control.pitch * 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); 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 */ if (gcs_link) {
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, /* send, add spaces so that string buffer is at least 10 chars long */
last_sensor_timestamp / 1000, mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
"ctrl0 ", last_sensor_timestamp / 1000,
actuators.control[0]); "ctrl0 ",
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, actuators.control[0]);
last_sensor_timestamp / 1000, mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
"ctrl1 ", last_sensor_timestamp / 1000,
actuators.control[1]); "ctrl1 ",
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, actuators.control[1]);
last_sensor_timestamp / 1000, mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
"ctrl2 ", last_sensor_timestamp / 1000,
actuators.control[2]); "ctrl2 ",
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, actuators.control[2]);
last_sensor_timestamp / 1000, mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
"ctrl3 ", last_sensor_timestamp / 1000,
actuators.control[3]); "ctrl3 ",
actuators.control[3]);
}
/* Only send in HIL mode */ /* Only send in HIL mode */
if (mavlink_hil_enabled) { if (mavlink_hil_enabled) {
apps/multirotor_att_control/multirotor_rate_control.c
+2 -2
View File
@@ -181,8 +181,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) { if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */ /* update parameters from storage */
parameters_update(&h, &p); parameters_update(&h, &p);
warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
} }
/* calculate current control outputs */ /* calculate current control outputs */