diff options
21 files changed, 129 insertions, 74 deletions
diff --git a/.gitignore b/.gitignore index 0372b60c8..3fdc1bf2a 100644 --- a/.gitignore +++ b/.gitignore @@ -24,4 +24,7 @@ Firmware.sublime-workspace Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ -NuttX
\ No newline at end of file +/NuttX +/Documentation/doxy.log +/Documentation/html/ +/Documentation/doxygen*objdb*tmp
\ No newline at end of file diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 4802ef818..241702811 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME = NO # disable (NO) the todo list. This list is created by putting \todo # commands in the documentation. -GENERATE_TODOLIST = YES +GENERATE_TODOLIST = NO # The GENERATE_TESTLIST tag can be used to enable (YES) or # disable (NO) the test list. This list is created by putting \test # commands in the documentation. -GENERATE_TESTLIST = YES +GENERATE_TESTLIST = NO # The GENERATE_BUGLIST tag can be used to enable (YES) or # disable (NO) the bug list. This list is created by putting \bug # commands in the documentation. -GENERATE_BUGLIST = YES +GENERATE_BUGLIST = NO # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or # disable (NO) the deprecated list. This list is created by putting @@ -569,7 +569,7 @@ WARN_LOGFILE = doxy.log # directories like "/usr/src/myproject". Separate the files or directories # with spaces. -INPUT = ../apps +INPUT = ../src # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is @@ -599,9 +599,8 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../dist/ \ - ../docs/html/ \ - html +EXCLUDE = ../src/modules/mathlib/CMSIS \ + ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or # directories that are symbolic links (a Unix filesystem feature) are excluded diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 6c9c137bb..b6b4546c2 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -53,6 +53,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> @@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&global_pos, 0, sizeof(global_pos)); struct manual_control_setpoint_s manual_sp; memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); @@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; @@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* control */ - - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + /* set manual setpoints if required */ + if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (vstatus.rc_signal_lost) { @@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - /* pass through flaps */ if (isfinite(manual_sp.flaps)) { actuators.control[4] = manual_sp.flaps; @@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[4] = 0.0f; } - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else { /* directly pass through values */ actuators.control[0] = manual_sp.roll; /* positive pitch means negative actuator -> pull up */ @@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } } } + + /* execute attitude control if requested */ + if (control_mode.flag_control_attitude_enabled) { + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; + + } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1ea06d419..1eb63a799 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -37,6 +37,10 @@ * Common object definitions without a better home. */ +/** + * @defgroup topics List of all uORB topics. + */ + #include <nuttx/config.h> #include <drivers/drv_orb_dev.h> diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 26b967237..cce2eafe1 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -55,11 +55,20 @@ /* control sets with pre-defined applications */ #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index 088c4fc8f..d7b404ad4 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -53,11 +53,20 @@ #define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_effective_s { uint64_t timestamp; float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_effective_0); ORB_DECLARE(actuator_controls_effective_1); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index bbe429073..30895ca83 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -52,12 +52,21 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ int noutputs; /**< valid outputs */ }; +/** + * @} + */ + /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs_0); ORB_DECLARE(actuator_outputs_1); diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h index a9d1b83fd..9253c787d 100644 --- a/src/modules/uORB/topics/debug_key_value.h +++ b/src/modules/uORB/topics/debug_key_value.h @@ -47,6 +47,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 00cf59b28..11332d7a7 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -52,10 +52,6 @@ #include "../uORB.h" /** - * @addtogroup topics @{ - */ - -/** * The number of ESCs supported. * Current (Q2/2013) we support 8 ESCs, */ @@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE { }; /** - * + * @addtogroup topics + * @{ + */ + +/** + * Electronic speed controller status. */ struct esc_status_s { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 253f444b3..978a3383a 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum NAV_CMD { NAV_CMD_WAYPOINT = 0, NAV_CMD_LOITER_TURN_COUNT, @@ -62,6 +57,11 @@ enum NAV_CMD { }; /** + * @addtogroup topics + * @{ + */ + +/** * Global position setpoint in WGS84 coordinates. * * This is the position the MAV is heading towards. If it of type loiter, diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2e895c59c..7901b930a 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -44,11 +44,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Off-board control inputs. * * Typically sent by a ground control station / joystick or by @@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; +/** + * @addtogroup topics + * @{ + */ + struct offboard_control_setpoint_s { uint64_t timestamp; diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h index 8f4be3b3f..a6ad8a131 100644 --- a/src/modules/uORB/topics/omnidirectional_flow.h +++ b/src/modules/uORB/topics/omnidirectional_flow.h @@ -46,6 +46,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 300e895c7..68964deb0 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -42,11 +42,20 @@ #include <stdint.h> #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + struct parameter_update_s { /** time at which the latest parameter was updated */ uint64_t timestamp; }; +/** + * @} + */ + ORB_DECLARE(parameter_update); #endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 2167e44a2..5a8580143 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, * leaving at a sane value of 15. @@ -85,6 +80,11 @@ enum RC_CHANNELS_FUNCTION RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; +/** + * @addtogroup topics + * @{ + */ + struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 9a76b5182..ad164555e 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum MAGNETOMETER_MODE { MAGNETOMETER_MODE_NORMAL = 0, MAGNETOMETER_MODE_POSITIVE_BIAS, @@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE { }; /** + * @addtogroup topics + * @{ + */ + +/** * Sensor readings in raw and SI-unit form. * * These values are read from the sensors. Raw values are in sensor-specific units, diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index c415e832e..cfe0bf69e 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,10 +50,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - */ - enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, @@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE }; /** + * @addtogroup topics + */ + +/** * State of individual sub systems */ struct subsystem_info_s { diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index f30852de5..828fb31cc 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE { TELEMETRY_STATUS_RADIO_TYPE_WIRE }; +/** + * @addtogroup topics + * @{ + */ + struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ @@ -62,6 +67,10 @@ struct telemetry_status_s { uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; +/** + * @} + */ + ORB_DECLARE(telemetry_status); #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index c31c81d0c..4380a5ee7 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -48,6 +48,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index fac571659..31ff014de 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Commands for commander app. * * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, @@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ }; +/** + * @addtogroup topics + * @{ + */ struct vehicle_command_s { diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 318abba89..8516b263f 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -60,8 +60,8 @@ */ struct vehicle_global_position_set_triplet_s { - bool previous_valid; - bool next_valid; + bool previous_valid; /**< flag indicating previous position is valid */ + bool next_valid; /**< flag indicating next position is valid */ struct vehicle_global_position_setpoint_s previous; struct vehicle_global_position_setpoint_s current; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index e7feaa98e..6690fb2be 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; +/** + * @addtogroup topics + * @{ + */ /** * state machine / state of vehicle. |