diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-21 17:37:00 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-21 17:37:00 +0100 |
commit | d9a31034133c62b1db4acf51459aacb1703f48d3 (patch) | |
tree | cbb3a1146dc5da1813a625387015da95a18a155e | |
parent | 4d1256e3f33ed78a6310695261f947273170dd56 (diff) | |
parent | e24c349d1d839dd7499645d17f58e93ba99a5588 (diff) | |
download | px4-firmware-d9a31034133c62b1db4acf51459aacb1703f48d3.tar.gz px4-firmware-d9a31034133c62b1db4acf51459aacb1703f48d3.tar.bz2 px4-firmware-d9a31034133c62b1db4acf51459aacb1703f48d3.zip |
Merge branch 'development' of github.com:PX4/Firmware into development
25 files changed, 468 insertions, 219 deletions
diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 88bcfec96..48399f1eb 100644 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -32,7 +32,7 @@ if exist(filePath, 'file') fileSize = fileInfo.bytes; fid = fopen(filePath, 'r'); - elements = int64(fileSize./(16*4+8))/4 + elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4)) for i=1:elements % timestamp @@ -79,6 +79,15 @@ if exist(filePath, 'file') % RotMatrix (9 channels) sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le'); + + % vicon position (6 channels) + sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le'); + + % actuator control effective (4 channels) + sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le'); + + % optical flow (6 values) + sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le'); end time_us = sensors(elements,1) - sensors(1,1); time_s = time_us*1e-6 diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 70b8ad6de..f15c74a22 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -45,6 +45,7 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> #include <systemlib/err.h> #include "ardrone_motor_control.h" @@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ float yaw_factor = 1.0f; + static bool initialized = false; + /* publish effective outputs */ + static struct actuator_controls_effective_s actuator_controls_effective; + static orb_advert_t actuator_controls_effective_pub; + if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; @@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { yaw_factor = 0.5f; + yaw_control *= yaw_factor; // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor); + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor); + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor); + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor); + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } for (int i = 0; i < 4; i++) { @@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls } } + /* publish effective outputs */ + actuator_controls_effective.control_effective[0] = roll_control; + actuator_controls_effective.control_effective[1] = pitch_control; + /* yaw output after limiting */ + actuator_controls_effective.control_effective[2] = yaw_control; + /* possible motor thrust limiting */ + actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; + + if (!initialized) { + /* advertise and publish */ + actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); + initialized = true; + } else { + /* already initialized, just publishing */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); + } + /* set the motor values */ /* scale up from 0..1 to 10..512) */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c3e825a86..7277e9fa4 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1393,7 +1393,7 @@ int commander_thread_main(int argc, char *argv[]) else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!"); state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index bf50ebad2..46793c89b 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -257,7 +257,9 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + // DO NOT abort mission + //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); } else { fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 18b290f99..334b95226 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -143,10 +143,10 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan; + float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h index f631c90a1..90d717d9f 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control.h +++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h @@ -42,26 +42,7 @@ #ifndef FIXEDWING_POS_CONTROL_H_ #define FIXEDWING_POS_CONTROL_H_ -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#endif - - -struct planned_path_segments_s { - bool segment_type; - double start_lat; // Start of line or center of arc - double start_lon; - double end_lat; - double end_lon; - float radius; // Radius of arc - float arc_start_bearing; // Bearing from center to start of arc - float arc_sweep; // Angle (radians) swept out by arc around center. - // Positive for clockwise, negative for counter-clockwise -}; float _wrap180(float bearing); diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 443048913..a70b9bd30 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -91,6 +91,19 @@ struct fw_pos_control_param_handles { }; +struct planned_path_segments_s { + bool segment_type; + double start_lat; // Start of line or center of arc + double start_lon; + double end_lat; + double end_lon; + float radius; // Radius of arc + float arc_start_bearing; // Bearing from center to start of arc + float arc_sweep; // Angle (radians) swept out by arc around center. + // Positive for clockwise, negative for counter-clockwise +}; + + /* Prototypes */ /* Internal Prototypes */ static int parameters_init(struct fw_pos_control_param_handles *h); @@ -177,9 +190,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); /* publish attitude setpoint */ - attitude_setpoint.roll_tait_bryan = 0.0f; - attitude_setpoint.pitch_tait_bryan = 0.0f; - attitude_setpoint.yaw_tait_bryan = 0.0f; + attitude_setpoint.roll_body = 0.0f; + attitude_setpoint.pitch_body = 0.0f; + attitude_setpoint.yaw_body = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ @@ -243,7 +256,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) global_sp_updated_set_once = true; psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", psi_track); + printf("psi_track: %0.4f\n", (double)psi_track); } /* Control */ @@ -265,21 +278,21 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards - if(delta_psi_c > 60.0f*M_DEG_TO_RAD) - delta_psi_c = 60.0f*M_DEG_TO_RAD; + if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) + delta_psi_c = 60.0f*M_DEG_TO_RAD_F; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD) - delta_psi_c = -60.0f*M_DEG_TO_RAD; + if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) + delta_psi_c = -60.0f*M_DEG_TO_RAD_F; float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; /* shift error to prevent wrapping issues */ - psi_e = _wrapPI(psi_e); + psi_e = _wrap_pi(psi_e); /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); // if (counter % 100 == 0) // printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); @@ -296,7 +309,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); } /*Publish the attitude setpoint */ @@ -378,64 +391,3 @@ int fixedwing_pos_control_main(int argc, char *argv[]) usage("unrecognized command"); exit(1); } - - -float _wrapPI(float bearing) -{ - - while (bearing > M_PI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing <= -M_PI_F) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -float _wrap2PI(float bearing) -{ - - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -float _wrap180(float bearing) -{ - - while (bearing > 180.0f) { - bearing = bearing - 360.0f; - } - - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - -float _wrap360(float bearing) -{ - - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; - } - - while (bearing < 0.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - - - - diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 3e485274e..6b4375445 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -206,6 +206,10 @@ handle_message(mavlink_message_t *msg) vicon_position.y = pos.y; vicon_position.z = pos.z; + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; + if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -335,6 +339,11 @@ handle_message(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* get a copy first, to prevent altering values that are not sent by the mavlink command */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); + mc.timestamp = rc_hil.timestamp; mc.roll = man.x / 1000.0f; mc.pitch = man.y / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 3ac229d73..4567a08f8 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -113,6 +113,7 @@ static void l_actuator_armed(struct listener *l); static void l_manual_control_setpoint(struct listener *l); static void l_vehicle_attitude_controls(struct listener *l); static void l_debug_key_value(struct listener *l); +static void l_optical_flow(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -134,6 +135,7 @@ struct listener listeners[] = { {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, + {l_optical_flow, &mavlink_subs.optical_flow, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -586,6 +588,17 @@ l_debug_key_value(struct listener *l) debug.value); } +void +l_optical_flow(struct listener *l) +{ + struct optical_flow_s flow; + + orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); +} + static void * uorb_receive_thread(void *arg) { @@ -710,6 +723,10 @@ uorb_receive_start(void) mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + /* --- FLOW SENSOR --- */ + mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 6860702d2..1b371e5cd 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -77,6 +77,7 @@ struct mavlink_subscriptions { int spg_sub; int debug_key_value; int input_rc_sub; + int optical_flow; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c index 45784eca7..87e2496d8 100644 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ b/apps/mavlink_onboard/mavlink_receiver.c @@ -145,7 +145,7 @@ handle_message(mavlink_message_t *msg) struct optical_flow_s f; - f.timestamp = flow.time_usec; + f.timestamp = hrt_absolute_time(); f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -161,6 +161,8 @@ handle_message(mavlink_message_t *msg) /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); } + + printf("GOT FLOW!\n"); } if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { @@ -288,7 +290,7 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); while (!thread_should_exit) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 7e390bc6c..ce1e52c1b 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -105,6 +105,7 @@ mc_thread_main(int argc, char *argv[]) memset(&rates_sp, 0, sizeof(rates_sp)); struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -151,10 +152,17 @@ mc_thread_main(int argc, char *argv[]) bool flag_control_attitude_enabled = false; bool flag_system_armed = false; + /* store if yaw position or yaw speed has been changed */ + bool control_yaw_position = true; + + /* store if we stopped a yaw movement */ + bool first_time_after_yaw_speed_control = true; + /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -205,28 +213,27 @@ mc_thread_main(int argc, char *argv[]) /** STEP 1: Define which input is the dominating control input */ if (state.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - // printf("thrust_rate=%8.4f\n",offboard_sp.p4); - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - // printf("thrust_att=%8.4f\n",offboard_sp.p4); - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* decide wether we want rate or position input */ - } + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; +// printf("thrust_rate=%8.4f\n",offboard_sp.p4); + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; +// printf("thrust_att=%8.4f\n",offboard_sp.p4); + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + /* decide wether we want rate or position input */ + } else if (state.flag_control_manual_enabled) { /* manual inputs, from RC control or joystick */ @@ -273,13 +280,30 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - /* only move setpoint if manual input is != 0 */ - // XXX turn into param - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; - } else if (manual.throttle <= 0.3f) { + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { att_sp.yaw_body = att.yaw; } + + /* only move setpoint if manual input is != 0 */ + + if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + control_yaw_position = true; + } + } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + } + att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } @@ -302,7 +326,8 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 8ffa90238..e94d7c55d 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static bool initialized = false; + static float yaw_error; + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -199,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* update parameters from storage */ parameters_update(&h, &p); - //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); @@ -216,9 +216,22 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); - /* control yaw rate */ - rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); + if(control_yaw_position) { + /* control yaw rate */ + + /* positive error: rotate to right, negative error, rotate to left (NED frame) */ + // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); + yaw_error = att_sp->yaw_body - att->yaw; + + if (yaw_error > M_PI_F) { + yaw_error -= M_TWOPI_F; + } else if (yaw_error < -M_PI_F) { + yaw_error += M_TWOPI_F; + } + + rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); + } rates_sp->thrust = att_sp->thrust; motor_skip_counter++; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index d9d3c0444..abc94d617 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -52,6 +52,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index bbaec3033..93f7085ae 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -209,6 +209,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + /* increase resilience to faulty control inputs */ + if (!isfinite(yaw_rate_control)) { + yaw_rate_control = 0.0f; + warnx("rej. NaN ctrl yaw"); + } actuators->control[0] = roll_control; actuators->control[1] = pitch_control; diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index eed72125a..d38bf9122 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -59,10 +59,13 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/optical_flow.h> #include <systemlib/systemlib.h> @@ -295,10 +298,13 @@ int sdlog_thread_main(int argc, char *argv[]) { struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; + struct actuator_controls_effective_s act_controls_effective; struct vehicle_command_s cmd; struct vehicle_local_position_s local_pos; struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; + struct vehicle_vicon_position_s vicon_pos; + struct optical_flow_s flow; } buf; memset(&buf, 0, sizeof(buf)); @@ -308,10 +314,13 @@ int sdlog_thread_main(int argc, char *argv[]) { int att_sub; int spa_sub; int act_0_sub; - int controls0_sub; + int controls_0_sub; + int controls_effective_0_sub; int local_pos_sub; int global_pos_sub; int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -353,8 +362,15 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- ACTUATOR CONTROL VALUE --- */ /* subscribe to ORB for actuator control */ - subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls0_sub; + subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.controls_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* subscribe to ORB for actuator control */ + subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.controls_effective_0_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -379,6 +395,20 @@ int sdlog_thread_main(int argc, char *argv[]) { fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VICON POSITION --- */ + /* subscribe to ORB for vicon position */ + subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + fds[fdsc_count].fd = subs.vicon_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- FLOW measurements --- */ + /* subscribe to ORB for flow measurements */ + subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); + fds[fdsc_count].fd = subs.flow_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -481,7 +511,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); /* copy actuator data into local buffer */ orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); @@ -489,6 +520,8 @@ int sdlog_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); #pragma pack(push, 1) struct { @@ -507,6 +540,9 @@ int sdlog_thread_main(int argc, char *argv[]) { int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] float attitude[3]; //pitch, roll, yaw [rad] float rotMatrix[9]; //unitvectors + float vicon[6]; + float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1] + float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, @@ -523,13 +559,16 @@ int sdlog_thread_main(int argc, char *argv[]) { .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]} + .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, + .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, + .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, + .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality} }; #pragma pack(pop) sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); - usleep(10000); //10000 corresponds in reality to ca. 76 Hz + usleep(3500); // roughly 150 Hz } fsync(sysvector_file); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eea51cc1e..466284a1b 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -151,6 +151,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ @@ -341,6 +342,7 @@ Sensors::Sensors() : _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), + _manual_control_sub(-1), /* publications */ _sensor_pub(-1), @@ -903,6 +905,9 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; + /* get a copy first, to prevent altering values */ + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) return; @@ -1023,6 +1028,7 @@ Sensors::task_main() _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -1052,20 +1058,18 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* advertise the manual_control topic */ - { - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - } + struct manual_control_setpoint_s manual_control; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; + manual_control.roll = 0.0f; + manual_control.pitch = 0.0f; + manual_control.yaw = 0.0f; + manual_control.throttle = 0.0f; + manual_control.aux1_cam_pan_flaps = 0.0f; + manual_control.aux2_cam_tilt = 0.0f; + manual_control.aux3_cam_zoom = 0.0f; + manual_control.aux4_cam_roll = 0.0f; + + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); /* advertise the rc topic */ { diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 2f4b37e79..6746e8ff3 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -60,14 +60,7 @@ static double cos_phi_1; static double lambda_0; static double scale; -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ phi_1 = lat_0 / 180.0 * M_PI; @@ -105,14 +98,7 @@ __EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lo } -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) { /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ double phi = lat / 180.0 * M_PI; @@ -135,15 +121,7 @@ __EXPORT static void map_projection_project(double lat, double lon, float *x, fl // printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); } -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) { /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ @@ -228,7 +206,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - theta = _wrapPI(theta); + theta = _wrap_pi(theta); return theta; } @@ -257,7 +235,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; - bearing_diff = _wrapPI(bearing_diff); + bearing_diff = _wrap_pi(bearing_diff); // Return past_end = true if past end point of line if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { @@ -270,10 +248,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); if (sin(bearing_diff) >= 0) { - crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F); + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); } else { - crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F); + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); } return_value = OK; @@ -380,22 +358,36 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d return return_value; } -float _wrapPI(float bearing) +__EXPORT float _wrap_pi(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing) || bearing == 0) { + return bearing; + } - while (bearing > M_PI_F) { - bearing = bearing - M_TWOPI_F; + int c = 0; + + while (bearing > M_PI_F && c < 30) { + bearing -= M_TWOPI_F; + c++; } - while (bearing <= -M_PI_F) { - bearing = bearing + M_TWOPI_F; + c = 0; + + while (bearing <= -M_PI_F && c < 30) { + bearing += M_TWOPI_F; + c++; } return bearing; } -float _wrap2PI(float bearing) +__EXPORT float _wrap_2pi(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing >= M_TWOPI_F) { bearing = bearing - M_TWOPI_F; @@ -408,8 +400,12 @@ float _wrap2PI(float bearing) return bearing; } -float _wrap180(float bearing) +__EXPORT float _wrap_180(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing > 180.0f) { bearing = bearing - 360.0f; @@ -422,8 +418,12 @@ float _wrap180(float bearing) return bearing; } -float _wrap360(float bearing) +__EXPORT float _wrap_360(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing >= 360.0f) { bearing = bearing - 360.0f; diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 7aad79a8c..0c0b5c533 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -54,24 +54,44 @@ struct crosstrack_error_s { float bearing; // Bearing in radians to closest point on line/arc } ; -__EXPORT static void map_projection_init(double lat_0, double lon_0); +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_init(double lat_0, double lon_0); -__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y); +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); -__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon); +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// - __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); -float _wrap180(float bearing); -float _wrap360(float bearing); -float _wrapPI(float bearing); -float _wrap2PI(float bearing); +__EXPORT float _wrap_180(float bearing); +__EXPORT float _wrap_360(float bearing); +__EXPORT float _wrap_pi(float bearing); +__EXPORT float _wrap_2pi(float bearing); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index b5e1d739c..dbee15050 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -95,6 +95,9 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_global_position_setpoint.h" ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); +#include "topics/vehicle_global_position_set_triplet.h" +ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); + #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); @@ -110,6 +113,7 @@ ORB_DEFINE(optical_flow, struct optical_flow_s); #include "topics/subsystem_info.h" ORB_DEFINE(subsystem_info, struct subsystem_info_s); +/* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); @@ -117,6 +121,13 @@ ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); +/* actuator controls, as set by actuators / mixers after limiting */ +#include "topics/actuator_controls_effective.h" +ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); + #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h new file mode 100644 index 000000000..3c72e4cb7 --- /dev/null +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H +#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_CONTROLS 8 +#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ + +struct actuator_controls_effective_s { + uint64_t timestamp; + float control_effective[NUM_ACTUATOR_CONTROLS]; +}; + +/* actuator control sets; this list can be expanded as more controllers emerge */ +ORB_DECLARE(actuator_controls_effective_0); +ORB_DECLARE(actuator_controls_effective_1); +ORB_DECLARE(actuator_controls_effective_2); +ORB_DECLARE(actuator_controls_effective_3); + +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) + +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index a7f5be708..1368cb716 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -48,13 +48,6 @@ * @{ */ -/** - * Defines how RC channels map to control inputs. - * - * The default mode on quadrotors and fixed wing is - * roll and pitch position of the right stick and - * throttle and yaw rate on the left stick - */ enum MANUAL_CONTROL_MODE { MANUAL_CONTROL_MODE_DIRECT = 0, diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 8663846fc..a7cda34a8 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,18 +56,13 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ - //float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ - float roll_body; /**< body angle in NED frame */ float pitch_body; /**< body angle in NED frame */ float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - //float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - //bool R_valid; /**< Set to true if rotation matrix is valid */ + float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + bool R_valid; /**< Set to true if rotation matrix is valid */ float thrust; /**< Thrust in Newton the power system should generate */ diff --git a/apps/uORB/topics/vehicle_global_position_set_triplet.h b/apps/uORB/topics/vehicle_global_position_set_triplet.h new file mode 100644 index 000000000..318abba89 --- /dev/null +++ b/apps/uORB/topics/vehicle_global_position_set_triplet.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position_setpoint.h + * Definition of the global WGS84 position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ +#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +#include "vehicle_global_position_setpoint.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint triplet in WGS84 coordinates. + * + * This are the three next waypoints (or just the next two or one). + */ +struct vehicle_global_position_set_triplet_s +{ + bool previous_valid; + bool next_valid; + + struct vehicle_global_position_setpoint_s previous; + struct vehicle_global_position_setpoint_s current; + struct vehicle_global_position_setpoint_s next; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position_set_triplet); + +#endif diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/apps/uORB/topics/vehicle_vicon_position.h index 91d933f44..0822fa89a 100644 --- a/apps/uORB/topics/vehicle_vicon_position.h +++ b/apps/uORB/topics/vehicle_vicon_position.h @@ -60,9 +60,9 @@ struct vehicle_vicon_position_s float x; /**< X positin in meters in NED earth-fixed frame */ float y; /**< X positin in meters in NED earth-fixed frame */ float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float vx; - float vy; - float vz; + float roll; + float pitch; + float yaw; // TODO Add covariances here |