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

Massive improvements in state machine, still tracing wrong throttle scaling in manual input path

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-13 18:53:37 +02:00
parent 56b3b46f75
commit 9014577aff
10 changed files with 268 additions and 167 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+28 -19
View File
@@ -314,7 +314,14 @@ int set_hil_on_off(uint8_t vehicle_mode)
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode)
{
//TODO: Make this correct
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
/* set mode flags independent of system state */
if (c_status->control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
switch (c_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT:
if (c_status->preflight_gyro_calibration || c_status->preflight_mag_calibration) {
@@ -579,8 +586,10 @@ static void *uorb_receiveloop(void *arg)
/* XXX this is seriously bad - should be an emergency */
} else {
int ifds = 0;
/* --- SENSORS RAW VALUE --- */
if (fds[0].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
@@ -596,7 +605,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- ATTITUDE VALUE --- */
if (fds[1].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
@@ -608,7 +617,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- GPS VALUE --- */
if (fds[2].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
/* GPS position */
@@ -622,7 +631,7 @@ static void *uorb_receiveloop(void *arg)
}
// /* --- ARDRONE CONTROL OUTPUTS --- */
// if (fds[3].revents & POLLIN) {
// if (fds[ifds++].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;
@@ -642,7 +651,7 @@ static void *uorb_receiveloop(void *arg)
// }
/* --- SYSTEM STATUS --- */
if (fds[4].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
/* enable or disable HIL */
@@ -658,7 +667,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- RC CHANNELS --- */
if (fds[5].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* 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 */
@@ -666,7 +675,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- FIXED WING CONTROL CHANNELS --- */
if (fds[6].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy fixed wing control into local buffer */
orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
/* send control output via MAVLink */
@@ -731,7 +740,7 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE GLOBAL POSITION --- */
if (fds[7].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
@@ -749,14 +758,14 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE LOCAL POSITION --- */
if (fds[8].revents & POLLIN) {
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
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) {
if (fds[ifds++].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;
@@ -765,14 +774,14 @@ static void *uorb_receiveloop(void *arg)
}
/* --- VEHICLE LOCAL SETPOINT --- */
if (fds[10].revents & POLLIN) {
if (fds[ifds++].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);
}
/* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[11].revents & POLLIN) {
if (fds[ifds++].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);
@@ -1148,15 +1157,15 @@ int mavlink_main(int argc, char *argv[])
//default values for arguments
char *uart_name = "/dev/ttyS0";
int baudrate = 115200;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n";
int baudrate = 57600;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */
int i;
for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
printf(commandline_usage, argv[0]);
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
@@ -1166,7 +1175,7 @@ int mavlink_main(int argc, char *argv[])
uart_name = argv[i + 1];
} else {
printf(commandline_usage, argv[0]);
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
}
@@ -1177,7 +1186,7 @@ int mavlink_main(int argc, char *argv[])
baudrate = atoi(argv[i + 1]);
} else {
printf(commandline_usage, argv[0]);
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
}
@@ -1223,7 +1232,7 @@ int mavlink_main(int argc, char *argv[])
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 2048 bytes */
pthread_attr_setstacksize(&uorb_attr, 4096);
pthread_attr_setstacksize(&uorb_attr, 5096);
pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
/* initialize waypoint manager */