fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 15:04:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sending back current position setpoints (global and local)

Browse Source
This commit is contained in:
Lorenz Meier 2012-08-07 08:36:20 +02:00
parent 7f2a63eb96
commit 2b09a7914f
1 changed files with 38 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
apps/mavlink/mavlink.c
View File

@ -385,7 +385,7 @@ static void *uorb_receiveloop(void *arg)
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 10;
const ssize_t fdsc = 15;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@ -397,6 +397,8 @@ static void *uorb_receiveloop(void *arg)
struct vehicle_attitude_s att;
struct vehicle_gps_position_s gps;
struct ardrone_control_s ar_control;
struct vehicle_local_position_setpoint_s local_sp;
struct vehicle_global_position_setpoint_s global_sp;
} buf;
/* --- SENSORS RAW VALUE --- */
@ -453,7 +455,7 @@ static void *uorb_receiveloop(void *arg)
/* struct already globally allocated */
/* subscribe to ORB for fixed wing control */
int fw_sub = orb_subscribe(ORB_ID(fixedwing_control));
orb_set_interval(fw_sub, 200); /* 5Hz updates */
orb_set_interval(fw_sub, 200); /* 5 Hz updates */
fds[fdsc_count].fd = fw_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@ -470,6 +472,24 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL SETPOINT VALUE --- */
/* subscribe to ORB for local setpoint */
/* struct already allocated */
int spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(spg_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spg_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL SETPOINT VALUE --- */
/* subscribe to ORB for local setpoint */
/* struct already allocated */
int spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(spl_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spl_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
unsigned int sensors_raw_counter = 0;
unsigned int attitude_counter = 0;
unsigned int gps_counter = 0;
@ -622,6 +642,21 @@ static void *uorb_receiveloop(void *arg)
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz);
}
/* --- VEHICLE GLOBAL SETPOINT --- */
if (fds[9].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw);
}
/* --- VEHICLE LOCAL SETPOINT --- */
if (fds[10].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp);
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
}
}
}
@ -669,7 +704,7 @@ void handleMessage(mavlink_message_t *msg)
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd_mavlink.param1 == 3.0f) {
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app
printf("[mavlink] Terminating .. \n");
fflush(stdout);