fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 02:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Streamlined ar drone interface, removed a lot of old cruft, preparing for generic multirotor control

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-12 01:25:41 +02:00
parent 22c1a03af7
commit 5adb691f89
26 changed files with 2339 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+42 -24
View File
@@ -71,6 +71,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include "waypoints.h"
#include "mavlink_log.h"
@@ -446,6 +447,7 @@ static void *uorb_receiveloop(void *arg)
struct ardrone_control_s ar_control;
struct vehicle_local_position_setpoint_s local_sp;
struct vehicle_global_position_setpoint_s global_sp;
struct vehicle_attitude_setpoint_s att_sp;
} buf;
/* --- SENSORS RAW VALUE --- */
@@ -472,13 +474,13 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ARDRONE CONTROL --- */
/* subscribe to ORB for AR.Drone controller outputs */
int ar_sub = orb_subscribe(ORB_ID(ardrone_control));
orb_set_interval(ar_sub, 200); /* 5Hz updates */
fds[fdsc_count].fd = ar_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
// /* --- ARDRONE CONTROL --- */
// /* subscribe to ORB for AR.Drone controller outputs */
// int ar_sub = orb_subscribe(ORB_ID(ardrone_control));
// orb_set_interval(ar_sub, 200); /* 5Hz updates */
// fds[fdsc_count].fd = ar_sub;
// fds[fdsc_count].events = POLLIN;
// fdsc_count++;
/* --- SYSTEM STATE --- */
/* struct already globally allocated */
@@ -537,6 +539,15 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE SETPOINT VALUE --- */
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
unsigned int sensors_raw_counter = 0;
unsigned int attitude_counter = 0;
unsigned int gps_counter = 0;
@@ -610,25 +621,25 @@ static void *uorb_receiveloop(void *arg)
gps_counter++;
}
/* --- ARDRONE CONTROL OUTPUTS --- */
if (fds[3].revents & POLLIN) {
/* copy ardrone control data into local buffer */
orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
uint64_t timestamp = buf.ar_control.timestamp;
float setpoint_roll = buf.ar_control.setpoint_attitude[0];
float setpoint_pitch = buf.ar_control.setpoint_attitude[1];
float setpoint_yaw = buf.ar_control.setpoint_attitude[2];
float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
// /* --- ARDRONE CONTROL OUTPUTS --- */
// if (fds[3].revents & POLLIN) {
// /* copy ardrone control data into local buffer */
// orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
// uint64_t timestamp = buf.ar_control.timestamp;
// float setpoint_roll = buf.ar_control.setpoint_attitude[0];
// float setpoint_pitch = buf.ar_control.setpoint_attitude[1];
// float setpoint_yaw = buf.ar_control.setpoint_attitude[2];
// float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
float control_roll = buf.ar_control.attitude_control_output[0];
float control_pitch = buf.ar_control.attitude_control_output[1];
float control_yaw = buf.ar_control.attitude_control_output[2];
// float control_roll = buf.ar_control.attitude_control_output[0];
// float control_pitch = buf.ar_control.attitude_control_output[1];
// float control_yaw = buf.ar_control.attitude_control_output[2];
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
}
// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
// }
/* --- SYSTEM STATUS --- */
if (fds[4].revents & POLLIN) {
@@ -759,6 +770,13 @@ static void *uorb_receiveloop(void *arg)
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);
}
/* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[11].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
}
}
}