diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
commit | 9014577aff02233e890d1f8eefc06471fca8b6d2 (patch) | |
tree | bd589542e278bad3e4978d2098587ffc76c29681 /apps/mavlink/mavlink.c | |
parent | 56b3b46f75c0b434932eecba2ac7207f84e2342e (diff) | |
download | px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.gz px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.bz2 px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.zip |
Massive improvements in state machine, still tracing wrong throttle scaling in manual input path
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 47 |
1 files changed, 28 insertions, 19 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 6745c0102..1e6156557 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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 */ |