diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 14:35:22 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 14:35:22 +0200 |
commit | fa9f145b08cace0f48c808cf307daf6cd43d9bd6 (patch) | |
tree | 06e8e7482f12ac072aaf8b2573632eee44118bcb | |
parent | 44ff4d4ee29f3da53f64b2f24b822f2f2f7032c0 (diff) | |
download | px4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.tar.gz px4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.tar.bz2 px4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.zip |
Fixed a bunch of issues in the arming state machine for multirotors, arming / disarming works fine now. Porting of various processes needed
-rw-r--r-- | apps/mavlink/mavlink.c | 285 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 43 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 2 | ||||
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 2 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 8 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_outputs.h | 2 |
7 files changed, 285 insertions, 59 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 84ea0aae2..1b23f6751 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -73,6 +73,8 @@ #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/manual_control_setpoint.h> #include "waypoints.h" #include "mavlink_log.h" @@ -129,9 +131,6 @@ static struct vehicle_command_s vcmd; static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; -static int sensor_sub = -1; -static int att_sub = -1; -static int global_pos_sub = -1; static int local_pos_sub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; @@ -146,6 +145,30 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; +static struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + bool initialized; +} mavlink_subs = { + .sensor_sub = 0, + .att_sub = 0, + .global_pos_sub = 0, + .act_0_sub = 0, + .act_1_sub = 0, + .act_2_sub = 0, + .act_3_sub = 0, + .gps_sub = 0, + .man_control_sp_sub = 0, + .initialized = false +}; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -459,23 +482,33 @@ static void *receiveloop(void *arg) return NULL; } -static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) +static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; switch (mavlink_msg_id) { case MAVLINK_MSG_ID_SCALED_IMU: /* senser sub triggers scaled IMU */ - orb_set_interval(sensor_sub, min_interval); + if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_RAW_IMU: /* senser sub triggers RAW IMU */ - orb_set_interval(sensor_sub, min_interval); + if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - orb_set_interval(att_sub, min_interval); + if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval); + break; + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval); + if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval); + if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval); + if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval); break; + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); default: /* not found */ ret = ERROR; @@ -494,13 +527,16 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) */ static void *uorb_receiveloop(void *arg) { + /* obtain reference to task's subscriptions */ + struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; + /* Set thread name */ prctl(PR_SET_NAME, "mavlink uORB", getpid()); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 25; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -515,28 +551,30 @@ static void *uorb_receiveloop(void *arg) struct vehicle_local_position_setpoint_s local_sp; struct vehicle_global_position_setpoint_s global_sp; struct vehicle_attitude_setpoint_s att_sp; + struct actuator_outputs_s act_outputs; + struct manual_control_setpoint_s man_control; } buf; /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = sensor_sub; + subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[fdsc_count].fd = subs->sensor_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ - att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(att_sub, 100); - fds[fdsc_count].fd = att_sub; + subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(subs->att_sub, 100); + fds[fdsc_count].fd = subs->att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS VALUE --- */ /* subscribe to ORB for attitude */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(gps_sub, 1000); /* 1Hz updates */ - fds[fdsc_count].fd = gps_sub; + subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */ + fds[fdsc_count].fd = subs->gps_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -577,7 +615,7 @@ static void *uorb_receiveloop(void *arg) /* --- GLOBAL POS VALUE --- */ /* struct already globally allocated and topic already subscribed */ - fds[fdsc_count].fd = global_pos_sub; + fds[fdsc_count].fd = subs->global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -608,12 +646,39 @@ static void *uorb_receiveloop(void *arg) /* --- ATTITUDE SETPOINT VALUE --- */ /* subscribe to ORB for attitude setpoint */ /* struct already allocated */ - int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */ fds[fdsc_count].fd = spa_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; + /** --- ACTUATOR OUTPUTS --- */ + subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs->act_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + fds[fdsc_count].fd = subs->act_1_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + fds[fdsc_count].fd = subs->act_2_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + fds[fdsc_count].fd = subs->act_3_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /** --- MAPPED MANUAL CONTROL INPUTS --- */ + subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + fds[fdsc_count].fd = subs->man_control_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* all subscriptions initialized, return success */ + subs->initialized = true; + unsigned int sensors_raw_counter = 0; unsigned int attitude_counter = 0; unsigned int gps_counter = 0; @@ -651,7 +716,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw); + orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); /* send raw imu data */ mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); @@ -667,7 +732,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); @@ -678,7 +743,7 @@ static void *uorb_receiveloop(void *arg) /* --- GPS VALUE --- */ if (fds[ifds++].revents & POLLIN) { /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps); + orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps); /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); @@ -801,7 +866,7 @@ static void *uorb_receiveloop(void *arg) /* --- VEHICLE GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos); uint64_t timestamp = global_pos.timestamp; int32_t lat = global_pos.lat; int32_t lon = global_pos.lon; @@ -845,6 +910,126 @@ static void *uorb_receiveloop(void *arg) 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); } + + /* --- ACTUATOR OUTPUTS 0 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 0 /* port 0 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + // 1 /* port 1 */, + // buf.act_outputs.output[ 8], + // buf.act_outputs.output[ 9], + // buf.act_outputs.output[10], + // buf.act_outputs.output[11], + // buf.act_outputs.output[12], + // buf.act_outputs.output[13], + // buf.act_outputs.output[14], + // buf.act_outputs.output[15]); + // } + } + + /* --- ACTUATOR OUTPUTS 1 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 3 /* port 3 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- ACTUATOR OUTPUTS 2 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 5 /* port 5 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- ACTUATOR OUTPUTS 3 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 7 /* port 7 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy local position data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, + buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1); + } } } @@ -891,7 +1076,8 @@ void handleMessage(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + 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 && ((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 @@ -1299,8 +1485,8 @@ int mavlink_thread_main(int argc, char *argv[]) /* topics to subscribe globally */ /* subscribe to ORB for global position */ - global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(global_pos_sub, 1000); /* 1Hz active updates */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ /* subscribe to ORB for local position */ local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */ @@ -1315,7 +1501,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_attr_init(&uorb_attr); /* Set stack size, needs more than 4000 bytes */ pthread_attr_setstacksize(&uorb_attr, 4096); - pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL); + pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1323,27 +1509,52 @@ int mavlink_thread_main(int argc, char *argv[]) uint16_t counter = 0; int lowspeed_counter = 0; + /* make sure all threads have registered their subscriptions */ + while (!mavlink_subs.initialized) { + usleep(500); + } + /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 921600) { /* 500 Hz / 2 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2); + /* 200 Hz / 5 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5); + /* 5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); } else if (baudrate >= 460800) { /* 250 Hz / 4 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5); + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100); + /* 1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 0.2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + /* 0.5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + /* 0.1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } while (!thread_should_exit) { @@ -1351,7 +1562,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (mavlink_exit_requested) break; /* get local and global position */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); /* check if waypoint has been reached against the last positions */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index df08ca75f..33e7ab7e7 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -78,13 +78,11 @@ static enum { static bool thread_should_exit; static int mc_task; +static bool motor_test_mode = false; static int mc_thread_main(int argc, char *argv[]) { - bool motor_test_mode = false; - - /* structures */ /* declare and safely initialize all structs */ struct vehicle_status_s state; memset(&state, 0, sizeof(state)); @@ -109,7 +107,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + //int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); // int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* rate-limit the attitude subscription to 200Hz to pace our loop */ @@ -120,8 +118,9 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - armed.armed = true; + armed.armed = false; orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -136,6 +135,8 @@ mc_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -150,7 +151,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; + att_sp.thrust = 0.1f; } else { if (state.state_machine == SYSTEM_STATE_MANUAL || state.state_machine == SYSTEM_STATE_GROUND_READY || @@ -159,28 +160,30 @@ mc_thread_main(int argc, char *argv[]) state.state_machine == SYSTEM_STATE_MISSION_ABORT || state.state_machine == SYSTEM_STATE_EMCY_LANDING) { att_sp.thrust = manual.throttle; + armed.armed = true; } else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) { /* immediately cut off motors */ - att_sp.thrust = 0.0f; + armed.armed = false; } else { /* limit motor throttle to zero for an unknown mode */ - att_sp.thrust = 0.0f; + armed.armed = false; } } multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); - //ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); /* publish the result */ + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } - printf("[multirotor att control] stopping.\n"); + printf("[multirotor att control] stopping, disarming motors.\n"); /* kill all outputs */ armed.armed = false; @@ -208,8 +211,9 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: multirotor_att_control [-m <mode>] {start|stop}\n"); + fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|stop}\n"); fprintf(stderr, " <mode> is 'rates' or 'attitude'\n"); + fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); exit(1); } @@ -220,7 +224,7 @@ int multirotor_att_control_main(int argc, char *argv[]) control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; - while ((ch = getopt(argc, argv, "m:")) != EOF) { + while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { case 'm': if (!strcmp(optarg, "rates")) { @@ -231,24 +235,33 @@ int multirotor_att_control_main(int argc, char *argv[]) usage("unrecognized -m value"); } optioncount += 2; + break; + case 't': + motor_test_mode = true; + optioncount += 1; + break; + case ':': + usage("missing parameter"); + break; default: + fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); } } argc -= optioncount; - argv += optioncount; + //argv += optioncount; if (argc < 1) usage("missing command"); - if (!strcmp(argv[1], "start")) { + if (!strcmp(argv[1+optioncount], "start")) { thread_should_exit = false; mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); exit(0); } - if (!strcmp(argv[1], "stop")) { + if (!strcmp(argv[1+optioncount], "stop")) { thread_should_exit = true; exit(0); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index b21a20ad1..886ce304e 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -239,4 +239,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[1] = pitch_control; actuators->control[2] = yaw_rate_control; actuators->control[3] = motor_thrust; + + motor_skip_counter++; } diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 3016cb01e..b5db823bd 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -212,7 +212,7 @@ FMUServo::task_main() orb_set_interval(_t_armed, 100); /* 10Hz update rate */ /* advertise the mixed control outputs */ - struct actuator_output_s outputs; + struct actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index b5875cb68..2739b3971 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -832,7 +832,7 @@ int sensors_thread_main(int argc, char *argv[]) if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f; + manual_control.throttle = (rc.chan[rc.function[THROTTLE]].scaled+1.0f)/2.0f; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 6ff66fceb..d6c00f996 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -116,7 +116,7 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_1, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_2, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_3, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index 63441a059..202f72b54 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -52,7 +52,7 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ -struct actuator_output_s { +struct actuator_outputs_s { float output[NUM_ACTUATOR_OUTPUTS]; }; |