From 4ccb72c249f0130e9853eaa74f33aabecaaff23e Mon Sep 17 00:00:00 2001 From: pgaskell Date: Fri, 18 Jan 2019 12:26:27 -0500 Subject: [PATCH 1/9] modify .gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 6985c0b..1d0e46a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -docs/html/* \ No newline at end of file +docs/html/* +.vscode/ \ No newline at end of file From 8e3a14369d65cd284e750d8b49790c32eceeae23 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Mon, 19 Aug 2019 07:56:47 -0400 Subject: [PATCH 2/9] Clang format --- .clang-format | 27 + include/feedback.h | 33 +- include/flight_mode.h | 120 ++-- include/input_manager.h | 54 +- include/log_manager.h | 139 ++--- include/mavlink_manager.h | 3 +- include/mix.h | 22 +- include/printf_manager.h | 7 +- include/rc_pilot_defs.h | 96 +-- include/setpoint_manager.h | 89 ++- include/settings.h | 189 +++--- include/state_estimator.h | 224 ++++--- include/thread_defs.h | 22 +- include/thrust_map.h | 14 +- src/feedback.c | 642 ++++++++++--------- src/input_manager.c | 467 +++++++------- src/log_manager.c | 596 +++++++++--------- src/main.c | 596 +++++++++--------- src/mavlink_manager.c | 123 ++-- src/mix.c | 421 +++++++------ src/printf_manager.c | 389 ++++++------ src/setpoint_manager.c | 511 +++++++-------- src/settings.c | 1204 +++++++++++++++++++----------------- src/state_estimator.c | 517 ++++++++-------- src/thrust_map.c | 318 +++++----- 25 files changed, 3475 insertions(+), 3348 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5d4e3a5 --- /dev/null +++ b/.clang-format @@ -0,0 +1,27 @@ +--- +BasedOnStyle: Google +BreakBeforeBraces: Allman +ColumnLimit: '100' +Cpp11BracedListStyle: 'true' +IndentWidth: '4' +Language: Cpp +NamespaceIndentation: None +ReflowComments: 'true' +SortIncludes: 'true' +SpaceAfterCStyleCast: 'false' +SpaceBeforeAssignmentOperators: 'true' +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: 'false' +SpacesInAngles: 'false' +SpacesInCStyleCastParentheses: 'false' +SpacesInContainerLiterals: 'false' +SpacesInParentheses: 'false' +SpacesInSquareBrackets: 'false' +Standard: Cpp11 +TabWidth: '4' +UseTab: Never +AlignAfterOpenBracket: DontAlign +AccessModifierOffset: -4 +PointerAlignment: Left +AllowShortFunctionsOnASingleLine: None +... diff --git a/include/feedback.h b/include/feedback.h index 601f4f7..4b3bb56 100644 --- a/include/feedback.h +++ b/include/feedback.h @@ -15,24 +15,25 @@ #ifndef FEEDBACK_H #define FEEDBACK_H -#include // for uint64_t #include +#include // for uint64_t /** * This is the state of the feedback loop. contains most recent values * reported by the feedback controller. Should only be written to by the * feedback controller after initialization. */ -typedef struct feedback_state_t{ - int initialized; ///< set to 1 after feedback_init(void) - arm_state_t arm_state; ///< actual arm state as reported by feedback controller - uint64_t arm_time_ns; ///< time since boot when controller was armed - uint64_t loop_index; ///< increases every time feedback loop runs - uint64_t last_step_ns; ///< last time controller has finished a step - - double u[6]; ///< siso controller outputs - double m[8]; ///< signals sent to motors after mapping -}feedback_state_t; +typedef struct feedback_state_t +{ + int initialized; ///< set to 1 after feedback_init(void) + arm_state_t arm_state; ///< actual arm state as reported by feedback controller + uint64_t arm_time_ns; ///< time since boot when controller was armed + uint64_t loop_index; ///< increases every time feedback loop runs + uint64_t last_step_ns; ///< last time controller has finished a step + + double u[6]; ///< siso controller outputs + double m[8]; ///< signals sent to motors after mapping +} feedback_state_t; extern feedback_state_t fstate; @@ -47,7 +48,6 @@ extern feedback_state_t fstate; */ int feedback_init(void); - /** * @brief marches feedback controller forward one step * @@ -78,7 +78,6 @@ int feedback_disarm(void); */ int feedback_arm(void); - /** * @brief Cleanup the feedback controller, freeing memory * @@ -86,10 +85,4 @@ int feedback_arm(void); */ int feedback_cleanup(void); - - - - - -#endif // FEEDBACK_H - +#endif // FEEDBACK_H diff --git a/include/flight_mode.h b/include/flight_mode.h index cb47052..1388705 100644 --- a/include/flight_mode.h +++ b/include/flight_mode.h @@ -8,68 +8,66 @@ /** * This is how the user interacts with the setpoint manager. */ -typedef enum flight_mode_t{ - /** - * test_bench mode does no feedback at all, it takes the raw user inputs - * and directly outputs to the motors. This could technically fly but - * would not be easy! Designed for confirming mixing matrix and motors - * are working. maps Z,Roll,Pitch,Yaw - */ - TEST_BENCH_4DOF, - /** - * test_bench mode does no feedback at all, it takes the raw user inputs - * and directly outputs to the motors. This could technically fly but - * would not be easy! Designed for confirming mixing matrix and motors - * are working. maps X,Y,Z,Yaw - */ - TEST_BENCH_6DOF, - /** - * user inputs translate directly to the throttle, roll, pitch, & yaw - * setpoints. No altitude feedback control. On 6DOF platforms the X & Y - * thrust directions are left at 0. - */ - DIRECT_THROTTLE_4DOF, - /** - * user inputs translate directly to the throttle, roll, pitch, & yaw - * setpoints. No altitude feedback control.Roll and pitch are left at 0 - */ - DIRECT_THROTTLE_6DOF, - /** - * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to - * hold altitude setpoint which is them moved up and down steadily based - * on user input. - */ - ALT_HOLD_4DOF, - /** - * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to - * hold altitude setpoint which is them moved up and down steadily based - * on user input. - */ - ALT_HOLD_6DOF, - /** - * Control sticks translate to velocity setpoints in horizontal - * translation X and Y. Yaw and Altitude are still position setpoints - * like alt_hold - */ - VELOCITY_CONTROL_4DOF, - /** - * Control sticks translate to velocity setpoints in horizontal - * translation X and Y. Yaw and Altitude are still position setpoints - * like alt_hold - */ - VELOCITY_CONTROL_6DOF, - /** - * PG: TODO: What do you intend for this mode? - */ - POSITION_CONTROL_4DOF, - /** - * PG: TODO: What do you intend for this mode? - */ - POSITION_CONTROL_6DOF +typedef enum flight_mode_t +{ + /** + * test_bench mode does no feedback at all, it takes the raw user inputs + * and directly outputs to the motors. This could technically fly but + * would not be easy! Designed for confirming mixing matrix and motors + * are working. maps Z,Roll,Pitch,Yaw + */ + TEST_BENCH_4DOF, + /** + * test_bench mode does no feedback at all, it takes the raw user inputs + * and directly outputs to the motors. This could technically fly but + * would not be easy! Designed for confirming mixing matrix and motors + * are working. maps X,Y,Z,Yaw + */ + TEST_BENCH_6DOF, + /** + * user inputs translate directly to the throttle, roll, pitch, & yaw + * setpoints. No altitude feedback control. On 6DOF platforms the X & Y + * thrust directions are left at 0. + */ + DIRECT_THROTTLE_4DOF, + /** + * user inputs translate directly to the throttle, roll, pitch, & yaw + * setpoints. No altitude feedback control.Roll and pitch are left at 0 + */ + DIRECT_THROTTLE_6DOF, + /** + * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to + * hold altitude setpoint which is them moved up and down steadily based + * on user input. + */ + ALT_HOLD_4DOF, + /** + * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to + * hold altitude setpoint which is them moved up and down steadily based + * on user input. + */ + ALT_HOLD_6DOF, + /** + * Control sticks translate to velocity setpoints in horizontal + * translation X and Y. Yaw and Altitude are still position setpoints + * like alt_hold + */ + VELOCITY_CONTROL_4DOF, + /** + * Control sticks translate to velocity setpoints in horizontal + * translation X and Y. Yaw and Altitude are still position setpoints + * like alt_hold + */ + VELOCITY_CONTROL_6DOF, + /** + * PG: TODO: What do you intend for this mode? + */ + POSITION_CONTROL_4DOF, + /** + * PG: TODO: What do you intend for this mode? + */ + POSITION_CONTROL_6DOF } flight_mode_t; - - - #endif \ No newline at end of file diff --git a/include/input_manager.h b/include/input_manager.h index 20aa897..9da03c9 100644 --- a/include/input_manager.h +++ b/include/input_manager.h @@ -9,45 +9,45 @@ #ifndef INPUT_MANAGER_H #define INPUT_MANAGER_H +#include // only for arm_state_t #include -#include // only for arm_state_t - /** * @brief determines how the dsm radio indicates an arm/disarm kill switch */ -typedef enum dsm_kill_mode_t{ - /** - * A dedicated channel is used as a kill switch. Carefully set the - * dsm_kill_ch and dsm_kill_pol channel and polarity settings. - */ - DSM_KILL_DEDICATED_SWITCH, - /** - * Some radios, such as Spektrum DXe have an ARM/DISARM switch which - * forces the throttle channel down below normal range to disarm. This - * frees up a channel for other use and is the preffered method. When - * using this mode, dsm_kill_ch and dsm_kill_pol are ignored. - */ - DSM_KILL_NEGATIVE_THROTTLE +typedef enum dsm_kill_mode_t +{ + /** + * A dedicated channel is used as a kill switch. Carefully set the + * dsm_kill_ch and dsm_kill_pol channel and polarity settings. + */ + DSM_KILL_DEDICATED_SWITCH, + /** + * Some radios, such as Spektrum DXe have an ARM/DISARM switch which + * forces the throttle channel down below normal range to disarm. This + * frees up a channel for other use and is the preffered method. When + * using this mode, dsm_kill_ch and dsm_kill_pol are ignored. + */ + DSM_KILL_NEGATIVE_THROTTLE } dsm_kill_mode_t; - /** * Represents current command by the user. This is populated by the * input_manager thread which decides to read from mavlink or DSM depending on * what it is receiving. */ -typedef struct user_input_t{ - int initialized; ///< set to 1 after input_manager_init(void) - flight_mode_t flight_mode; ///< this is the user commanded flight_mode. - int input_active; ///< nonzero indicates some user control is coming in - arm_state_t requested_arm_mode; ///< set to ARMED after arming sequence is entered. +typedef struct user_input_t +{ + int initialized; ///< set to 1 after input_manager_init(void) + flight_mode_t flight_mode; ///< this is the user commanded flight_mode. + int input_active; ///< nonzero indicates some user control is coming in + arm_state_t requested_arm_mode; ///< set to ARMED after arming sequence is entered. - // All sticks scaled from -1 to 1 - double thr_stick; ///< positive forward - double yaw_stick; ///< positive to the right, CW yaw - double roll_stick; ///< positive to the right - double pitch_stick; ///< positive forward + // All sticks scaled from -1 to 1 + double thr_stick; ///< positive forward + double yaw_stick; ///< positive to the right, CW yaw + double roll_stick; ///< positive to the right + double pitch_stick; ///< positive forward } user_input_t; extern user_input_t user_input; @@ -73,4 +73,4 @@ int input_manager_init(void); */ int input_manager_cleanup(void); -#endif // INPUT_MANAGER_H +#endif // INPUT_MANAGER_H diff --git a/include/log_manager.h b/include/log_manager.h index 8b61988..bd1834d 100644 --- a/include/log_manager.h +++ b/include/log_manager.h @@ -9,7 +9,6 @@ #ifndef LOG_MANAGER_H #define LOG_MANAGER_H - /** * Struct containing all possible values that could be writen to the log. For * each log entry you wish to create, fill in an instance of this and pass to @@ -17,77 +16,76 @@ * Currently feedback.c populates all values and log_manager.c only writes the * values enabled in the settings file. */ -typedef struct log_entry_t{ - /** @name index, always printed */ - ///@{ - uint64_t loop_index; // timing - uint64_t last_step_ns; - ///@} - - /** @name sensors */ - ///@{ - double v_batt; - double alt_bmp_raw; - double gyro_roll; - double gyro_pitch; - double gyro_yaw; - double accel_X; - double accel_Y; - double accel_Z; - ///@} - - /** @name state estimate */ - ///@{ - double roll; - double pitch; - double yaw; - double X; - double Y; - double Z; - double Xdot; - double Ydot; - double Zdot; - ///@} - - /** @name setpoint */ - ///@{ - double sp_roll; - double sp_pitch; - double sp_yaw; - double sp_X; - double sp_Y; - double sp_Z; - double sp_Xdot; - double sp_Ydot; - double sp_Zdot; - ///@} - - /** @name orthogonal control outputs */ - ///@{ - double u_roll; - double u_pitch; - double u_yaw; - double u_X; - double u_Y; - double u_Z; - ///@} - - /** @name motor signals */ - ///@{ - double mot_1; - double mot_2; - double mot_3; - double mot_4; - double mot_5; - double mot_6; - double mot_7; - double mot_8; - ///@} +typedef struct log_entry_t +{ + /** @name index, always printed */ + ///@{ + uint64_t loop_index; // timing + uint64_t last_step_ns; + ///@} + + /** @name sensors */ + ///@{ + double v_batt; + double alt_bmp_raw; + double gyro_roll; + double gyro_pitch; + double gyro_yaw; + double accel_X; + double accel_Y; + double accel_Z; + ///@} + + /** @name state estimate */ + ///@{ + double roll; + double pitch; + double yaw; + double X; + double Y; + double Z; + double Xdot; + double Ydot; + double Zdot; + ///@} + + /** @name setpoint */ + ///@{ + double sp_roll; + double sp_pitch; + double sp_yaw; + double sp_X; + double sp_Y; + double sp_Z; + double sp_Xdot; + double sp_Ydot; + double sp_Zdot; + ///@} + + /** @name orthogonal control outputs */ + ///@{ + double u_roll; + double u_pitch; + double u_yaw; + double u_X; + double u_Y; + double u_Z; + ///@} + + /** @name motor signals */ + ///@{ + double mot_1; + double mot_2; + double mot_3; + double mot_4; + double mot_5; + double mot_6; + double mot_7; + double mot_8; + ///@} } log_entry_t; - - /** * @brief creates a new csv log file and starts the background thread. * @@ -95,7 +93,6 @@ typedef struct log_entry_t{ */ int log_manager_init(void); - /** * @brief quickly add new data to local buffer * @@ -115,4 +112,4 @@ int log_manager_add_new(); */ int log_manager_cleanup(void); -#endif // LOG_MANAGER_H +#endif // LOG_MANAGER_H diff --git a/include/mavlink_manager.h b/include/mavlink_manager.h index 73e04d5..cd5b9b2 100644 --- a/include/mavlink_manager.h +++ b/include/mavlink_manager.h @@ -7,7 +7,6 @@ #ifndef MAVLINK_MANAGER_H #define MAVLINK_MANAGER_H - /** * @brief Starts the mavlink manager * @@ -22,4 +21,4 @@ int mavlink_manager_init(void); */ int mavlink_manager_cleanup(void); -#endif // MAVLINK_MANAGER_H +#endif // MAVLINK_MANAGER_H diff --git a/include/mix.h b/include/mix.h index 8b9994b..eae2fb4 100644 --- a/include/mix.h +++ b/include/mix.h @@ -14,21 +14,22 @@ #ifndef MIXING_MATRIX_H #define MIXING_MATRIX_H -#define MAX_INPUTS 6 ///< up to 6 control inputs (roll,pitch,yaw,z,x,y) -#define MAX_ROTORS 8 ///< up to 8 rotors +#define MAX_INPUTS 6 ///< up to 6 control inputs (roll,pitch,yaw,z,x,y) +#define MAX_ROTORS 8 ///< up to 8 rotors /** * @brief enum for possible mixing matrices defined here * * possible rotor configurations, see mixing_matrix_defs.h */ -typedef enum rotor_layout_t{ - LAYOUT_4X, - LAYOUT_4PLUS, - LAYOUT_6X, - LAYOUT_8X, - LAYOUT_6DOF_ROTORBITS, - LAYOUT_6DOF_5INCH_MONOCOQUE +typedef enum rotor_layout_t +{ + LAYOUT_4X, + LAYOUT_4PLUS, + LAYOUT_6X, + LAYOUT_8X, + LAYOUT_6DOF_ROTORBITS, + LAYOUT_6DOF_5INCH_MONOCOQUE } rotor_layout_t; /** @@ -105,5 +106,4 @@ int mix_check_saturation(int ch, double* mot, double* min, double* max); */ int mix_add_input(double u, int ch, double* mot); - -#endif // MIXING_MATRIX_H \ No newline at end of file +#endif // MIXING_MATRIX_H \ No newline at end of file diff --git a/include/printf_manager.h b/include/printf_manager.h index 39c7043..24c910c 100644 --- a/include/printf_manager.h +++ b/include/printf_manager.h @@ -5,7 +5,6 @@ * separate thread printing data to the console for debugging. */ - #ifndef PRINTF_MANAGER_H #define PRINTF_MANAGER_H @@ -19,7 +18,6 @@ */ int printf_init(void); - /** * @brief Waits for the printf manager thread to exit. * @@ -27,7 +25,6 @@ int printf_init(void); */ int printf_cleanup(void); - /** * @brief Only used by printf_manager right now, but could be useful * elsewhere. @@ -38,6 +35,4 @@ int printf_cleanup(void); */ int print_flight_mode(flight_mode_t mode); - - -#endif //PRINTF_MANAGER_H +#endif // PRINTF_MANAGER_H diff --git a/include/rc_pilot_defs.h b/include/rc_pilot_defs.h index b626b79..5d43dbe 100644 --- a/include/rc_pilot_defs.h +++ b/include/rc_pilot_defs.h @@ -11,82 +11,82 @@ * @brief ARMED or DISARMED to indicate if the feedback controller is * allowed to output to the motors */ -typedef enum arm_state_t{ - DISARMED, - ARMED +typedef enum arm_state_t +{ + DISARMED, + ARMED } arm_state_t; // Speed of feedback loop -#define FEEDBACK_HZ 200 -#define DT 0.005 +#define FEEDBACK_HZ 200 +#define DT 0.005 -//IMU Parameters -#define IMU_PRIORITY 51 +// IMU Parameters +#define IMU_PRIORITY 51 #define I2C_BUS 2 #define GPIO_INT_PIN_CHIP 3 -#define GPIO_INT_PIN_PIN 21 +#define GPIO_INT_PIN_PIN 21 // top safety -#define ARM_TIP_THRESHOLD 0.2 ///< radians from level to allow arming sequence -#define TIP_ANGLE 1.5 ///< radiands of roll or pitch to consider tipped over +#define ARM_TIP_THRESHOLD 0.2 ///< radians from level to allow arming sequence +#define TIP_ANGLE 1.5 ///< radiands of roll or pitch to consider tipped over // math constants -#define GRAVITY 9.80665 ///< one G m/s^2 +#define GRAVITY 9.80665 ///< one G m/s^2 // order of control inputs // throttle(Z), roll, pitch, YAW, sideways (X),forward(Y) -#define VEC_X 0 -#define VEC_Y 1 -#define VEC_Z 2 -#define VEC_ROLL 3 -#define VEC_PITCH 4 -#define VEC_YAW 5 +#define VEC_X 0 +#define VEC_Y 1 +#define VEC_Z 2 +#define VEC_ROLL 3 +#define VEC_PITCH 4 +#define VEC_YAW 5 // user control parameters -#define MAX_YAW_RATE 2.5 // rad/s -#define MAX_ROLL_SETPOINT 0.2 // rad -#define MAX_PITCH_SETPOINT 0.2 // rad -#define MAX_CLIMB_RATE 1.0 // m/s -#define YAW_DEADZONE 0.02 -#define THROTTLE_DEADZONE 0.02 -#define SOFT_START_SECONDS 1.0 // controller soft start seconds -#define ALT_CUTOFF_FREQ 2.0 -#define BMP_RATE_DIV 10 // optionally sample bmp less frequently than mpu +#define MAX_YAW_RATE 2.5 // rad/s +#define MAX_ROLL_SETPOINT 0.2 // rad +#define MAX_PITCH_SETPOINT 0.2 // rad +#define MAX_CLIMB_RATE 1.0 // m/s +#define YAW_DEADZONE 0.02 +#define THROTTLE_DEADZONE 0.02 +#define SOFT_START_SECONDS 1.0 // controller soft start seconds +#define ALT_CUTOFF_FREQ 2.0 +#define BMP_RATE_DIV 10 // optionally sample bmp less frequently than mpu // controller absolute limits -#define MAX_ROLL_COMPONENT 0.4 -#define MAX_PITCH_COMPONENT 0.4 -#define MAX_YAW_COMPONENT 0.4 -#define MAX_X_COMPONENT 1.0 -#define MAX_Y_COMPONENT 1.0 +#define MAX_ROLL_COMPONENT 0.4 +#define MAX_PITCH_COMPONENT 0.4 +#define MAX_YAW_COMPONENT 0.4 +#define MAX_X_COMPONENT 1.0 +#define MAX_Y_COMPONENT 1.0 /** * MAX_THRUST_COMPONENT is really "lowest power state" or idle value. Note that * after the thrust mapping a different value will actually be sent to the motors. * The sign is inverted because these are control values in NED coordinates */ -#define MAX_THRUST_COMPONENT -0.05 -#define MIN_THRUST_COMPONENT -0.75 +#define MAX_THRUST_COMPONENT -0.05 +#define MIN_THRUST_COMPONENT -0.75 // Files -#define LOG_DIR "/home/debian/rc_pilot_logs/" - +#define LOG_DIR "/home/debian/rc_pilot_logs/" // for future modes, not used yet -#define LAND_TIMEOUT 0.3 -#define DISARM_TIMEOUT 4.0 +#define LAND_TIMEOUT 0.3 +#define DISARM_TIMEOUT 4.0 // terminal emulator control sequences -#define WRAP_DISABLE "\033[?7l" -#define WRAP_ENABLE "\033[?7h" -#define KNRM "\x1B[0m" // "normal" to return to default after colour -#define KRED "\x1B[31m" -#define KGRN "\x1B[32m" -#define KYEL "\x1B[33m" -#define KBLU "\x1B[34m" -#define KMAG "\x1B[35m" -#define KCYN "\x1B[36m" -#define KWHT "\x1B[37m" +#define WRAP_DISABLE "\033[?7l" +#define WRAP_ENABLE "\033[?7h" +#define KNRM "\x1B[0m" // "normal" to return to default after colour +#define KRED "\x1B[31m" +#define KGRN "\x1B[32m" +#define KYEL "\x1B[33m" +#define KBLU "\x1B[34m" +#define KMAG "\x1B[35m" +#define KCYN "\x1B[36m" +#define KWHT "\x1B[37m" //#define DEBUG -#endif // RC_PILOT_DEFS_H \ No newline at end of file +#endif // RC_PILOT_DEFS_H \ No newline at end of file diff --git a/include/setpoint_manager.h b/include/setpoint_manager.h index 26c2fc2..2d38961 100644 --- a/include/setpoint_manager.h +++ b/include/setpoint_manager.h @@ -25,54 +25,54 @@ * and primarily read in by fly_controller. May also be read by printf_manager * and log_manager for telemetry */ -typedef struct setpoint_t{ +typedef struct setpoint_t +{ + /** @name general */ + ///< @{ + int initialized; ///< set to 1 once setpoint manager has initialized + int en_6dof; ///< enable 6DOF control features + ///< @} - /** @name general */ - ///< @{ - int initialized; ///< set to 1 once setpoint manager has initialized - int en_6dof; ///< enable 6DOF control features - ///< @} + /** @name direct passthrough + * user inputs tranlate directly to mixing matrix + */ + ///< @{ + double Z_throttle; ///< used only when altitude controller disabled + double X_throttle; ///< only used when 6dof is enabled, positive forward + double Y_throttle; ///< only used when 6dof is enabled, positive right + double roll_throttle; ///< only used when roll_pitch_yaw controllers are disbaled + double pitch_throttle; ///< only used when roll_pitch_yaw controllers are disbaled + double yaw_throttle; ///< only used when roll_pitch_yaw controllers are disbaled + ///< @} - /** @name direct passthrough - * user inputs tranlate directly to mixing matrix - */ - ///< @{ - double Z_throttle; ///< used only when altitude controller disabled - double X_throttle; ///< only used when 6dof is enabled, positive forward - double Y_throttle; ///< only used when 6dof is enabled, positive right - double roll_throttle; ///< only used when roll_pitch_yaw controllers are disbaled - double pitch_throttle; ///< only used when roll_pitch_yaw controllers are disbaled - double yaw_throttle; ///< only used when roll_pitch_yaw controllers are disbaled - ///< @} + /** @name attitude setpoint */ + ///< @{ + int en_rpy_ctrl; ///< enable the roll pitch yaw controllers + double roll; ///< roll angle (positive tip right) (rad) + double pitch; ///< pitch angle (positive tip back) (rad) + double yaw; ///< glabal yaw angle, positive left + double yaw_dot; ///< desired rate of change in yaw rad/s + ///< @} - /** @name attitude setpoint */ - ///< @{ - int en_rpy_ctrl; ///< enable the roll pitch yaw controllers - double roll; ///< roll angle (positive tip right) (rad) - double pitch; ///< pitch angle (positive tip back) (rad) - double yaw; ///< glabal yaw angle, positive left - double yaw_dot; ///< desired rate of change in yaw rad/s - ///< @} + /** @name altitude */ + ///< @{ + int en_Z_ctrl; ///< enable altitude feedback. + double Z; ///< vertical distance from where controller was armed + double Z_dot; ///< vertical velocity m/s^2, remember Z points down + ///< @} - /** @name altitude */ - ///< @{ - int en_Z_ctrl; ///< enable altitude feedback. - double Z; ///< vertical distance from where controller was armed - double Z_dot; ///< vertical velocity m/s^2, remember Z points down - ///< @} + /** @name horizontal velocity setpoint */ + ///< @{ + int en_XY_vel_ctrl; + double X_dot; + double Y_dot; + ///< @} - /** @name horizontal velocity setpoint */ - ///< @{ - int en_XY_vel_ctrl; - double X_dot; - double Y_dot; - ///< @} - - /** @name horizontal velocity setpoint */ - ///< @{ - int en_XY_pos_ctrl; - double X; - double Y; + /** @name horizontal velocity setpoint */ + ///< @{ + int en_XY_pos_ctrl; + double X; + double Y; } setpoint_t; extern setpoint_t setpoint; @@ -99,5 +99,4 @@ int setpoint_manager_update(void); */ int setpoint_manager_cleanup(void); - -#endif // SETPOINT_MANAGER_H +#endif // SETPOINT_MANAGER_H diff --git a/include/settings.h b/include/settings.h index ba03659..5c61de6 100644 --- a/include/settings.h +++ b/include/settings.h @@ -11,106 +11,104 @@ #include #include -#include -#include #include +#include #include - - +#include /** * Configuration settings read from the json settings file and passed to most * threads as they initialize. */ -typedef struct settings_t{ - /** @name File details */ - char name[128]; ///< string declaring the name of the settings file - ///@} - - /**@name warings */ - ///@{ - int warnings_en; - ///@} - - /** @name physical parameters */ - ///@{ - int num_rotors; - rotor_layout_t layout; - int dof; - thrust_map_t thrust_map; - double v_nominal; - int enable_magnetometer; // we suggest leaving as 0 (mag OFF) - ///@} - - /** @name flight modes */ - ///@{ - int num_dsm_modes; - flight_mode_t flight_mode_1; - flight_mode_t flight_mode_2; - flight_mode_t flight_mode_3; - ///@} - - - /** @name dsm radio config */ - ///@{ - int dsm_thr_ch; - int dsm_thr_pol; - int dsm_roll_ch; - int dsm_roll_pol; - int dsm_pitch_ch; - int dsm_pitch_pol; - int dsm_yaw_ch; - int dsm_yaw_pol; - int dsm_mode_ch; - int dsm_mode_pol; - dsm_kill_mode_t dsm_kill_mode; - int dsm_kill_ch; - int dsm_kill_pol; - ///@} - - /** @name printf settings */ - ///@{ - int printf_arm; - int printf_altitude; - int printf_rpy; - int printf_sticks; - int printf_setpoint; - int printf_u; - int printf_motors; - int printf_mode; - ///@} - - /** @name log settings */ - ///@{ - int enable_logging; - int log_sensors; - int log_state; - int log_setpoint; - int log_control_u; - int log_motor_signals; - ///@} - - /** @name mavlink stuff */ - ///@{ - char dest_ip[24]; - uint8_t my_sys_id; - uint16_t mav_port; - - /** @name feedback controllers */ - ///@{ - rc_filter_t roll_controller; - rc_filter_t pitch_controller; - rc_filter_t yaw_controller; - rc_filter_t altitude_controller; - rc_filter_t horiz_vel_ctrl_4dof; - rc_filter_t horiz_vel_ctrl_6dof; - rc_filter_t horiz_pos_ctrl_4dof; - rc_filter_t horiz_pos_ctrl_6dof; - double max_XY_velocity; - double max_Z_velocity; - ///@} - -}settings_t; +typedef struct settings_t +{ + /** @name File details */ + char name[128]; ///< string declaring the name of the settings file + ///@} + + /**@name warings */ + ///@{ + int warnings_en; + ///@} + + /** @name physical parameters */ + ///@{ + int num_rotors; + rotor_layout_t layout; + int dof; + thrust_map_t thrust_map; + double v_nominal; + int enable_magnetometer; // we suggest leaving as 0 (mag OFF) + ///@} + + /** @name flight modes */ + ///@{ + int num_dsm_modes; + flight_mode_t flight_mode_1; + flight_mode_t flight_mode_2; + flight_mode_t flight_mode_3; + ///@} + + /** @name dsm radio config */ + ///@{ + int dsm_thr_ch; + int dsm_thr_pol; + int dsm_roll_ch; + int dsm_roll_pol; + int dsm_pitch_ch; + int dsm_pitch_pol; + int dsm_yaw_ch; + int dsm_yaw_pol; + int dsm_mode_ch; + int dsm_mode_pol; + dsm_kill_mode_t dsm_kill_mode; + int dsm_kill_ch; + int dsm_kill_pol; + ///@} + + /** @name printf settings */ + ///@{ + int printf_arm; + int printf_altitude; + int printf_rpy; + int printf_sticks; + int printf_setpoint; + int printf_u; + int printf_motors; + int printf_mode; + ///@} + + /** @name log settings */ + ///@{ + int enable_logging; + int log_sensors; + int log_state; + int log_setpoint; + int log_control_u; + int log_motor_signals; + ///@} + + /** @name mavlink stuff */ + ///@{ + char dest_ip[24]; + uint8_t my_sys_id; + uint16_t mav_port; + + /** @name feedback controllers */ + ///@{ + rc_filter_t roll_controller; + rc_filter_t pitch_controller; + rc_filter_t yaw_controller; + rc_filter_t altitude_controller; + rc_filter_t horiz_vel_ctrl_4dof; + rc_filter_t horiz_vel_ctrl_6dof; + rc_filter_t horiz_pos_ctrl_4dof; + rc_filter_t horiz_pos_ctrl_6dof; + double max_XY_velocity; + double max_Z_velocity; + ///@} + +} settings_t; /** * settings are external, so just include this header and read from it @@ -124,7 +122,6 @@ extern settings_t settings; */ int settings_load_from_file(char* path); - /** * @brief Only used in debug mode. Prints settings to console * @@ -132,6 +129,4 @@ int settings_load_from_file(char* path); */ int settings_print(void); - - -#endif // SETTINGS_H +#endif // SETTINGS_H diff --git a/include/state_estimator.h b/include/state_estimator.h index fa45a62..8bd9cc6 100644 --- a/include/state_estimator.h +++ b/include/state_estimator.h @@ -11,9 +11,9 @@ #ifndef STATE_ESTIMATOR_H #define STATE_ESTIMATOR_H -#include // for uint64_t +#include #include - #include +#include // for uint64_t /** * This is the output from the state estimator. It contains raw sensor values @@ -28,117 +28,114 @@ * - Positive Pitch back about Y * - Positive Yaw right about Z */ -typedef struct state_estimate_t{ - - int initialized; - - /** @name IMU (accel gyro) - * Normalized Quaternion is straight from the DMP but converted to NED - * coordinates. Tait-Bryan angles roll pitch and yaw angles are then - * converted from the quaternion. - * the roll_pitch_yaw values in the taid bryan angles tb_imu are bounded - * by +-pi since they come straight from the quaternion. the state estimator - * keeps track of these rotations and generates continuous_yaw which is - * unbounded and keeps track of multiple rotations. This provides a continuously - * differentiable variable with no jumps between +-pi - */ - ///@{ - double gyro[3]; ///< gyro roll pitch yaw (rad/s) - double accel[3]; ///< accel XYZ NED coordinates (m/s^2) - double quat_imu[4]; ///< DMP normalized quaternion - double tb_imu[3]; ///< tait bryan roll pitch yaw angle (rad) - double imu_continuous_yaw; ///< continuous yaw from imu only (multiple turns) - ///@} - - /** @name IMU (magnetometer) - * these values are only set when magnetometer is enabled in settings. - * right now these aren't used and we don't suggest turning the magnetometer on - */ - ///@{ - double mag[3]; ///< magnetometer XYZ NED coordinates () - double mag_heading_raw; ///< raw compass heading - double mag_heading; ///< compass heading filtered with IMU - double mag_heading_continuous; - double quat_mag[4]; ///< quaterion filtered - double tb_mag[3]; ///< roll pitch yaw with magetometer heading fixed (rad) - ///@} - - /** @name selected values for feedback - * these are copoies of other values in this state estimate used for feedback - * this is done so we can easily chose which source to get feedback from (mag or no mag) - */ - ///@{ - double roll; - double pitch; - double yaw; - double continuous_yaw; ///< keeps increasing/decreasing aboce +-2pi - double X; - double Y; - double Z; - ///@} - - - /** @name filtered data from IMU & barometer - * Altitude estimates from kalman filter fusing IMU and BMP data. - * Alttitude, velocity, and acceleration are in units of m, m/s, m/s^2 - * Note this is altitude so positive is upwards unlike the NED - * coordinate frame that has Z pointing down. - */ - ///@{ - double bmp_pressure_raw;///< raw barometer pressure in Pascals - double alt_bmp_raw; ///< altitude estimate using only bmp from sea level (m) - double alt_bmp; ///< altitude estimate using kalman filter (IMU & bmp) - double alt_bmp_vel; ///< z velocity estimate using kalman filter (IMU & bmp) - double alt_bmp_accel; ///< z accel estimate using kalman filter (IMU & bmp) - ///@} - - /** @name Motion Capture data - * As mocap drop in and out the mocap_running flag will turn on and off. - * Old values will remain readable after mocap drops out. - */ - ///@{ - int mocap_running; ///< 1 if motion capture data is recent and valid - uint64_t mocap_timestamp_ns; ///< timestamp of last received packet in nanoseconds since boot - double pos_mocap[3]; ///< position in mocap frame, converted to NED if necessary - double quat_mocap[4]; ///< UAV orientation according to mocap - double tb_mocap[3]; ///< Tait-Bryan angles according to mocap - int is_active; ///< TODO used by mavlink manager, purpose unclear... (pg) - ///@} - - /** @name Global Position Estimate - * This is the global estimated position, velocity, and acceleration - * output of a kalman filter which filters accelerometer, DMP attitude, - * and mocap data. If mocap is not available, barometer will be used. - * - * global values are in the mocap's frame for position control. - * relative values are in a frame who's origin is at the position where - * the feedback controller is armed. Without mocap data the filter will - * assume altitude from the barometer and accelerometer, and position - * estimate will have steady state gain of zero to prevent runaway. - */ - ///@{ - double pos_global[3]; - double vel_global[3]; - double accel_global[3]; - double pos_relative[3]; - double vel_relative[3]; - double accel_relative[3]; - ///@} - - /** @name Other */ - ///@{ - double v_batt_raw; ///< main battery pack voltage (v) - double v_batt_lp; ///< main battery pack voltage with low pass filter (v) - double bmp_temp; ///< temperature of barometer - ///@} - -}state_estimate_t; +typedef struct state_estimate_t +{ + int initialized; + + /** @name IMU (accel gyro) + * Normalized Quaternion is straight from the DMP but converted to NED + * coordinates. Tait-Bryan angles roll pitch and yaw angles are then + * converted from the quaternion. + * the roll_pitch_yaw values in the taid bryan angles tb_imu are bounded + * by +-pi since they come straight from the quaternion. the state estimator + * keeps track of these rotations and generates continuous_yaw which is + * unbounded and keeps track of multiple rotations. This provides a continuously + * differentiable variable with no jumps between +-pi + */ + ///@{ + double gyro[3]; ///< gyro roll pitch yaw (rad/s) + double accel[3]; ///< accel XYZ NED coordinates (m/s^2) + double quat_imu[4]; ///< DMP normalized quaternion + double tb_imu[3]; ///< tait bryan roll pitch yaw angle (rad) + double imu_continuous_yaw; ///< continuous yaw from imu only (multiple turns) + ///@} + + /** @name IMU (magnetometer) + * these values are only set when magnetometer is enabled in settings. + * right now these aren't used and we don't suggest turning the magnetometer on + */ + ///@{ + double mag[3]; ///< magnetometer XYZ NED coordinates () + double mag_heading_raw; ///< raw compass heading + double mag_heading; ///< compass heading filtered with IMU + double mag_heading_continuous; + double quat_mag[4]; ///< quaterion filtered + double tb_mag[3]; ///< roll pitch yaw with magetometer heading fixed (rad) + ///@} + + /** @name selected values for feedback + * these are copoies of other values in this state estimate used for feedback + * this is done so we can easily chose which source to get feedback from (mag or no mag) + */ + ///@{ + double roll; + double pitch; + double yaw; + double continuous_yaw; ///< keeps increasing/decreasing aboce +-2pi + double X; + double Y; + double Z; + ///@} + + /** @name filtered data from IMU & barometer + * Altitude estimates from kalman filter fusing IMU and BMP data. + * Alttitude, velocity, and acceleration are in units of m, m/s, m/s^2 + * Note this is altitude so positive is upwards unlike the NED + * coordinate frame that has Z pointing down. + */ + ///@{ + double bmp_pressure_raw; ///< raw barometer pressure in Pascals + double alt_bmp_raw; ///< altitude estimate using only bmp from sea level (m) + double alt_bmp; ///< altitude estimate using kalman filter (IMU & bmp) + double alt_bmp_vel; ///< z velocity estimate using kalman filter (IMU & bmp) + double alt_bmp_accel; ///< z accel estimate using kalman filter (IMU & bmp) + ///@} + + /** @name Motion Capture data + * As mocap drop in and out the mocap_running flag will turn on and off. + * Old values will remain readable after mocap drops out. + */ + ///@{ + int mocap_running; ///< 1 if motion capture data is recent and valid + uint64_t mocap_timestamp_ns; ///< timestamp of last received packet in nanoseconds since boot + double pos_mocap[3]; ///< position in mocap frame, converted to NED if necessary + double quat_mocap[4]; ///< UAV orientation according to mocap + double tb_mocap[3]; ///< Tait-Bryan angles according to mocap + int is_active; ///< TODO used by mavlink manager, purpose unclear... (pg) + ///@} + + /** @name Global Position Estimate + * This is the global estimated position, velocity, and acceleration + * output of a kalman filter which filters accelerometer, DMP attitude, + * and mocap data. If mocap is not available, barometer will be used. + * + * global values are in the mocap's frame for position control. + * relative values are in a frame who's origin is at the position where + * the feedback controller is armed. Without mocap data the filter will + * assume altitude from the barometer and accelerometer, and position + * estimate will have steady state gain of zero to prevent runaway. + */ + ///@{ + double pos_global[3]; + double vel_global[3]; + double accel_global[3]; + double pos_relative[3]; + double vel_relative[3]; + double accel_relative[3]; + ///@} + + /** @name Other */ + ///@{ + double v_batt_raw; ///< main battery pack voltage (v) + double v_batt_lp; ///< main battery pack voltage with low pass filter (v) + double bmp_temp; ///< temperature of barometer + ///@} + +} state_estimate_t; extern state_estimate_t state_estimate; extern rc_mpu_data_t mpu_data; - - /** * @brief Initial setup of the state estimator * @@ -148,7 +145,6 @@ extern rc_mpu_data_t mpu_data; */ int state_estimator_init(void); - /** * @brief March state estimator forward one step * @@ -158,7 +154,6 @@ int state_estimator_init(void); */ int state_estimator_march(void); - /** * @brief jobs the state estimator must do after feedback_controller * @@ -168,7 +163,6 @@ int state_estimator_march(void); */ int state_estimator_jobs_after_feedback(void); - /** * @brief Cleanup the state estimator, freeing memory * @@ -176,8 +170,4 @@ int state_estimator_jobs_after_feedback(void); */ int state_estimator_cleanup(void); - - - -#endif // STATE_ESTIMATOR_H - +#endif // STATE_ESTIMATOR_H diff --git a/include/thread_defs.h b/include/thread_defs.h index 4752b12..3b9e245 100644 --- a/include/thread_defs.h +++ b/include/thread_defs.h @@ -4,16 +4,16 @@ #define THREAD_DEFS_H // thread speeds, prioritites, and close timeouts -#define INPUT_MANAGER_HZ 20 -#define INPUT_MANAGER_PRI 80 -#define INPUT_MANAGER_TOUT 0.5 -#define LOG_MANAGER_HZ 20 -#define LOG_MANAGER_PRI 50 -#define LOG_MANAGER_TOUT 2.0 -#define PRINTF_MANAGER_HZ 20 -#define PRINTF_MANAGER_PRI 60 -#define PRINTF_MANAGER_TOUT 0.5 -#define BUTTON_EXIT_CHECK_HZ 10 -#define BUTTON_EXIT_TIME_S 2 +#define INPUT_MANAGER_HZ 20 +#define INPUT_MANAGER_PRI 80 +#define INPUT_MANAGER_TOUT 0.5 +#define LOG_MANAGER_HZ 20 +#define LOG_MANAGER_PRI 50 +#define LOG_MANAGER_TOUT 2.0 +#define PRINTF_MANAGER_HZ 20 +#define PRINTF_MANAGER_PRI 60 +#define PRINTF_MANAGER_TOUT 0.5 +#define BUTTON_EXIT_CHECK_HZ 10 +#define BUTTON_EXIT_TIME_S 2 #endif \ No newline at end of file diff --git a/include/thrust_map.h b/include/thrust_map.h index 1116f8c..402b49d 100644 --- a/include/thrust_map.h +++ b/include/thrust_map.h @@ -5,7 +5,6 @@ * separate thread printing data to the console for debugging. */ - #ifndef THRUST_MAP_H #define THRUST_MAP_H @@ -14,14 +13,14 @@ * * the user may select from the following preconfigured thrust maps */ -typedef enum thrust_map_t{ +typedef enum thrust_map_t +{ LINEAR_MAP, - MN1806_1400KV_4S, - F20_2300KV_2S, - RX2206_4S + MN1806_1400KV_4S, + F20_2300KV_2S, + RX2206_4S } thrust_map_t; - /** * @brief Check the thrust map for validity and populate data arrays. * @@ -29,7 +28,6 @@ typedef enum thrust_map_t{ */ int thrust_map_init(thrust_map_t map); - /** * @brief Corrects the motor signal m for non-linear thrust curve in place. * @@ -40,4 +38,4 @@ int thrust_map_init(thrust_map_t map); */ double map_motor_signal(double m); -#endif // THRUST_MAP_H +#endif // THRUST_MAP_H diff --git a/src/feedback.c b/src/feedback.c index 9eaca7a..d0dca89 100644 --- a/src/feedback.c +++ b/src/feedback.c @@ -3,373 +3,371 @@ * */ -#include #include +#include #include #include -#include #include -#include -#include +#include #include #include +#include #include +#include #include -#include +#include +#include #include #include -#include #include -#include +#include #include -#define TWO_PI (M_PI*2.0) +#define TWO_PI (M_PI * 2.0) -feedback_state_t fstate; // extern variable in feedback.h +feedback_state_t fstate; // extern variable in feedback.h // keep original controller gains for scaling later static double D_roll_gain_orig, D_pitch_gain_orig, D_yaw_gain_orig, D_Z_gain_orig; - // filters -static rc_filter_t D_roll = RC_FILTER_INITIALIZER; -static rc_filter_t D_pitch = RC_FILTER_INITIALIZER; -static rc_filter_t D_yaw = RC_FILTER_INITIALIZER; -static rc_filter_t D_Z = RC_FILTER_INITIALIZER; -static rc_filter_t D_Xdot_4 = RC_FILTER_INITIALIZER; -static rc_filter_t D_Xdot_6 = RC_FILTER_INITIALIZER; -static rc_filter_t D_X_4 = RC_FILTER_INITIALIZER; -static rc_filter_t D_X_6 = RC_FILTER_INITIALIZER; -static rc_filter_t D_Ydot_4 = RC_FILTER_INITIALIZER; -static rc_filter_t D_Ydot_6 = RC_FILTER_INITIALIZER; -static rc_filter_t D_Y_4 = RC_FILTER_INITIALIZER; -static rc_filter_t D_Y_6 = RC_FILTER_INITIALIZER; - +static rc_filter_t D_roll = RC_FILTER_INITIALIZER; +static rc_filter_t D_pitch = RC_FILTER_INITIALIZER; +static rc_filter_t D_yaw = RC_FILTER_INITIALIZER; +static rc_filter_t D_Z = RC_FILTER_INITIALIZER; +static rc_filter_t D_Xdot_4 = RC_FILTER_INITIALIZER; +static rc_filter_t D_Xdot_6 = RC_FILTER_INITIALIZER; +static rc_filter_t D_X_4 = RC_FILTER_INITIALIZER; +static rc_filter_t D_X_6 = RC_FILTER_INITIALIZER; +static rc_filter_t D_Ydot_4 = RC_FILTER_INITIALIZER; +static rc_filter_t D_Ydot_6 = RC_FILTER_INITIALIZER; +static rc_filter_t D_Y_4 = RC_FILTER_INITIALIZER; +static rc_filter_t D_Y_6 = RC_FILTER_INITIALIZER; static int __send_motor_stop_pulse(void) { - int i; - if(settings.num_rotors>8){ - printf("ERROR: set_motors_to_idle: too many rotors\n"); - return -1; - } - for(i=0;i 8) + { + printf("ERROR: set_motors_to_idle: too many rotors\n"); + return -1; + } + for (i = 0; i < settings.num_rotors; i++) + { + fstate.m[i] = -0.1; + rc_servo_send_esc_pulse_normalized(i + 1, -0.1); + } + return 0; } static void __rpy_init(void) { - // get controllers from settings - - rc_filter_duplicate(&D_roll, settings.roll_controller); - rc_filter_duplicate(&D_pitch, settings.pitch_controller); - rc_filter_duplicate(&D_yaw, settings.yaw_controller); - - #ifdef DEBUG - printf("ROLL CONTROLLER:\n"); - rc_filter_print(D_roll); - printf("PITCH CONTROLLER:\n"); - rc_filter_print(D_pitch); - printf("YAW CONTROLLER:\n"); - rc_filter_print(D_yaw); - #endif - - // save original gains as we will scale these by battery voltage later - D_roll_gain_orig = D_roll.gain; - D_pitch_gain_orig = D_pitch.gain; - D_yaw_gain_orig = D_yaw.gain; - - // enable saturation. these limits will be changed late but we need to - // enable now so that soft start can also be enabled - rc_filter_enable_saturation(&D_roll, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT); - rc_filter_enable_saturation(&D_pitch, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT); - rc_filter_enable_saturation(&D_yaw, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT); - // enable soft start - rc_filter_enable_soft_start(&D_roll, SOFT_START_SECONDS); - rc_filter_enable_soft_start(&D_pitch, SOFT_START_SECONDS); - rc_filter_enable_soft_start(&D_yaw, SOFT_START_SECONDS); + // get controllers from settings + + rc_filter_duplicate(&D_roll, settings.roll_controller); + rc_filter_duplicate(&D_pitch, settings.pitch_controller); + rc_filter_duplicate(&D_yaw, settings.yaw_controller); + +#ifdef DEBUG + printf("ROLL CONTROLLER:\n"); + rc_filter_print(D_roll); + printf("PITCH CONTROLLER:\n"); + rc_filter_print(D_pitch); + printf("YAW CONTROLLER:\n"); + rc_filter_print(D_yaw); +#endif + + // save original gains as we will scale these by battery voltage later + D_roll_gain_orig = D_roll.gain; + D_pitch_gain_orig = D_pitch.gain; + D_yaw_gain_orig = D_yaw.gain; + + // enable saturation. these limits will be changed late but we need to + // enable now so that soft start can also be enabled + rc_filter_enable_saturation(&D_roll, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT); + rc_filter_enable_saturation(&D_pitch, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT); + rc_filter_enable_saturation(&D_yaw, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT); + // enable soft start + rc_filter_enable_soft_start(&D_roll, SOFT_START_SECONDS); + rc_filter_enable_soft_start(&D_pitch, SOFT_START_SECONDS); + rc_filter_enable_soft_start(&D_yaw, SOFT_START_SECONDS); } - int feedback_disarm(void) { - fstate.arm_state = DISARMED; - // set LEDs - rc_led_set(RC_LED_RED,1); - rc_led_set(RC_LED_GREEN,0); - return 0; + fstate.arm_state = DISARMED; + // set LEDs + rc_led_set(RC_LED_RED, 1); + rc_led_set(RC_LED_GREEN, 0); + return 0; } - int feedback_arm(void) { - if(fstate.arm_state==ARMED){ - printf("WARNING: trying to arm when controller is already armed\n"); - return -1; - } - // start a new log file every time controller is armed, this may take some - // time so do it before touching anything else - if(settings.enable_logging) log_manager_init(); - // get the current time - fstate.arm_time_ns = rc_nanos_since_boot(); - // reset the index - fstate.loop_index = 0; - // when swapping from direct throttle to altitude control, the altitude - // controller needs to know the last throttle input for smooth transition - // TODO: Reinitialize altitude bias - //last_en_alt_ctrl = 0; - //last_usr_thr = MIN_Z_COMPONENT; - // yaw estimator can be zero'd too - // TODO: Reinitialize yaw estimate - //num_yaw_spins = 0; - //last_yaw = -mpu_data.fused_TaitBryan[TB_YAW_Z]; // minus because NED coordinates - // zero out all filters - rc_filter_reset(&D_roll); - rc_filter_reset(&D_pitch); - rc_filter_reset(&D_yaw); - rc_filter_reset(&D_Z); - - // prefill filters with current error - rc_filter_prefill_inputs(&D_roll, -state_estimate.roll); - rc_filter_prefill_inputs(&D_pitch, -state_estimate.pitch); - // set LEDs - rc_led_set(RC_LED_RED,0); - rc_led_set(RC_LED_GREEN,1); - // last thing is to flag as armed - fstate.arm_state = ARMED; - return 0; + if (fstate.arm_state == ARMED) + { + printf("WARNING: trying to arm when controller is already armed\n"); + return -1; + } + // start a new log file every time controller is armed, this may take some + // time so do it before touching anything else + if (settings.enable_logging) log_manager_init(); + // get the current time + fstate.arm_time_ns = rc_nanos_since_boot(); + // reset the index + fstate.loop_index = 0; + // when swapping from direct throttle to altitude control, the altitude + // controller needs to know the last throttle input for smooth transition + // TODO: Reinitialize altitude bias + // last_en_alt_ctrl = 0; + // last_usr_thr = MIN_Z_COMPONENT; + // yaw estimator can be zero'd too + // TODO: Reinitialize yaw estimate + // num_yaw_spins = 0; + // last_yaw = -mpu_data.fused_TaitBryan[TB_YAW_Z]; // minus because NED coordinates + // zero out all filters + rc_filter_reset(&D_roll); + rc_filter_reset(&D_pitch); + rc_filter_reset(&D_yaw); + rc_filter_reset(&D_Z); + + // prefill filters with current error + rc_filter_prefill_inputs(&D_roll, -state_estimate.roll); + rc_filter_prefill_inputs(&D_pitch, -state_estimate.pitch); + // set LEDs + rc_led_set(RC_LED_RED, 0); + rc_led_set(RC_LED_GREEN, 1); + // last thing is to flag as armed + fstate.arm_state = ARMED; + return 0; } - - int feedback_init(void) { - double tmp; - - __rpy_init(); - - rc_filter_duplicate(&D_Z, settings.altitude_controller); - rc_filter_duplicate(&D_Xdot_4, settings.horiz_vel_ctrl_4dof); - rc_filter_duplicate(&D_Xdot_6, settings.horiz_vel_ctrl_6dof); - rc_filter_duplicate(&D_X_4, settings.horiz_pos_ctrl_4dof); - rc_filter_duplicate(&D_X_6, settings.horiz_pos_ctrl_6dof); - rc_filter_duplicate(&D_Ydot_4, settings.horiz_vel_ctrl_4dof); - rc_filter_duplicate(&D_Ydot_6, settings.horiz_vel_ctrl_6dof); - rc_filter_duplicate(&D_Y_4, settings.horiz_pos_ctrl_4dof); - rc_filter_duplicate(&D_Y_6, settings.horiz_pos_ctrl_6dof); - - - #ifdef DEBUG - printf("ALTITUDE CONTROLLER:\n"); - rc_filter_print(D_Z); - #endif - - D_Z_gain_orig = D_Z.gain; - - rc_filter_enable_saturation(&D_Z, -1.0, 1.0); - rc_filter_enable_soft_start(&D_Z, SOFT_START_SECONDS); - // make sure everything is disarmed them start the ISR - feedback_disarm(); - fstate.initialized=1; - - return 0; + double tmp; + + __rpy_init(); + + rc_filter_duplicate(&D_Z, settings.altitude_controller); + rc_filter_duplicate(&D_Xdot_4, settings.horiz_vel_ctrl_4dof); + rc_filter_duplicate(&D_Xdot_6, settings.horiz_vel_ctrl_6dof); + rc_filter_duplicate(&D_X_4, settings.horiz_pos_ctrl_4dof); + rc_filter_duplicate(&D_X_6, settings.horiz_pos_ctrl_6dof); + rc_filter_duplicate(&D_Ydot_4, settings.horiz_vel_ctrl_4dof); + rc_filter_duplicate(&D_Ydot_6, settings.horiz_vel_ctrl_6dof); + rc_filter_duplicate(&D_Y_4, settings.horiz_pos_ctrl_4dof); + rc_filter_duplicate(&D_Y_6, settings.horiz_pos_ctrl_6dof); + +#ifdef DEBUG + printf("ALTITUDE CONTROLLER:\n"); + rc_filter_print(D_Z); +#endif + + D_Z_gain_orig = D_Z.gain; + + rc_filter_enable_saturation(&D_Z, -1.0, 1.0); + rc_filter_enable_soft_start(&D_Z, SOFT_START_SECONDS); + // make sure everything is disarmed them start the ISR + feedback_disarm(); + fstate.initialized = 1; + + return 0; } - - - - - int feedback_march(void) { - int i; - double tmp, min, max; - double u[6], mot[8]; - log_entry_t new_log; - static int last_en_Z_ctrl = 0; - - // Disarm if rc_state is somehow paused without disarming the controller. - // This shouldn't happen if other threads are working properly. - if(rc_get_state()!=RUNNING && fstate.arm_state==ARMED){ - feedback_disarm(); - } - - // check for a tipover - if(fabs(state_estimate.roll)>TIP_ANGLE || fabs(state_estimate.pitch)>TIP_ANGLE){ - feedback_disarm(); - printf("\n TIPOVER DETECTED \n"); - } - - // if not running or not armed, keep the motors in an idle state - if(rc_get_state()!=RUNNING || fstate.arm_state==DISARMED){ - __send_motor_stop_pulse(); - return 0; - } - - // We are about to start marching the individual SISO controllers forward. - // Start by zeroing out the motors signals then add from there. - for(i=0;i<8;i++) mot[i] = 0.0; - for(i=0;i<6;i++) u[i] = 0.0; - - - /*************************************************************************** - * Throttle/Altitude Controller - * - * If transitioning from direct throttle to altitude control, prefill the - * filter with current throttle input to make smooth transition. This is also - * true if taking off for the first time in altitude mode as arm_controller - * sets up last_en_Z_ctrl and last_usr_thr every time controller arms - ***************************************************************************/ - // run altitude controller if enabled - // this needs work... - // we need to: - // find hover thrust and correct from there - // this code does not work a.t.m. - if(setpoint.en_Z_ctrl){ - if(last_en_Z_ctrl == 0){ - setpoint.Z = state_estimate.alt_bmp; // set altitude setpoint to current altitude - rc_filter_reset(&D_Z); - tmp = -setpoint.Z_throttle / (cos(state_estimate.roll)*cos(state_estimate.pitch)); - rc_filter_prefill_outputs(&D_Z, tmp); - last_en_Z_ctrl = 1; - } - D_Z.gain = D_Z_gain_orig * settings.v_nominal/state_estimate.v_batt_lp; - tmp = rc_filter_march(&D_Z, -setpoint.Z+state_estimate.alt_bmp); //altitude is positive but +Z is down - rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); - u[VEC_Z] = tmp / cos(state_estimate.roll)*cos(state_estimate.pitch); - mix_add_input(u[VEC_Z], VEC_Z, mot); - last_en_Z_ctrl = 1; - } - // else use direct throttle - else{ - - // compensate for tilt - tmp = setpoint.Z_throttle / (cos(state_estimate.roll)*cos(state_estimate.pitch)); - //printf("throttle: %f\n",tmp); - rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); - u[VEC_Z] = tmp; - mix_add_input(u[VEC_Z], VEC_Z, mot); - } - - /*************************************************************************** - * Roll Pitch Yaw controllers, only run if enabled - ***************************************************************************/ - if(setpoint.en_rpy_ctrl){ - // Roll - mix_check_saturation(VEC_ROLL, mot, &min, &max); - if(max>MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; - if(min<-MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; - rc_filter_enable_saturation(&D_roll, min, max); - D_roll.gain = D_roll_gain_orig * settings.v_nominal/state_estimate.v_batt_lp; - u[VEC_ROLL] = rc_filter_march(&D_roll, setpoint.roll - state_estimate.roll); - mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); - - // pitch - mix_check_saturation(VEC_PITCH, mot, &min, &max); - if(max>MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; - if(min<-MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; - rc_filter_enable_saturation(&D_pitch, min, max); - D_pitch.gain = D_pitch_gain_orig * settings.v_nominal/state_estimate.v_batt_lp; - u[VEC_PITCH] = rc_filter_march(&D_pitch, setpoint.pitch - state_estimate.pitch); - mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); - - // Yaw - // if throttle stick is down (waiting to take off) keep yaw setpoint at - // current heading, otherwide update by yaw rate - mix_check_saturation(VEC_YAW, mot, &min, &max); - if(max>MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; - if(min<-MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; - rc_filter_enable_saturation(&D_yaw, min, max); - D_yaw.gain = D_yaw_gain_orig * settings.v_nominal/state_estimate.v_batt_lp; - u[VEC_YAW] = rc_filter_march(&D_yaw, setpoint.yaw - state_estimate.yaw); - mix_add_input(u[VEC_YAW], VEC_YAW, mot); - } - // otherwise direct throttle to roll pitch yaw - else{ - // roll - mix_check_saturation(VEC_ROLL, mot, &min, &max); - if(max>MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; - if(min<-MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; - u[VEC_ROLL] = setpoint.roll_throttle; - rc_saturate_double(&u[VEC_ROLL], min, max); - mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); - - // pitch - mix_check_saturation(VEC_PITCH, mot, &min, &max); - if(max>MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; - if(min<-MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; - u[VEC_PITCH] = setpoint.pitch_throttle; - rc_saturate_double(&u[VEC_PITCH], min, max); - mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); - - // YAW - mix_check_saturation(VEC_YAW, mot, &min, &max); - if(max>MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; - if(min<-MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; - u[VEC_YAW] = setpoint.yaw_throttle; - rc_saturate_double(&u[VEC_YAW], min, max); - mix_add_input(u[VEC_YAW], VEC_YAW, mot); - } - - // for 6dof systems, add X and Y - if(setpoint.en_6dof){ - // X - mix_check_saturation(VEC_X, mot, &min, &max); - if(max>MAX_X_COMPONENT) max = MAX_X_COMPONENT; - if(min<-MAX_X_COMPONENT) min = -MAX_X_COMPONENT; - u[VEC_X] = setpoint.X_throttle; - rc_saturate_double(&u[VEC_X], min, max); - mix_add_input(u[VEC_X], VEC_X, mot); - - // Y - mix_check_saturation(VEC_Y, mot, &min, &max); - if(max>MAX_Y_COMPONENT) max = MAX_Y_COMPONENT; - if(min<-MAX_Y_COMPONENT) min = -MAX_Y_COMPONENT; - u[VEC_Y] = setpoint.Y_throttle; - rc_saturate_double(&u[VEC_Y], min, max); - mix_add_input(u[VEC_Y], VEC_Y, mot); - } - - /*************************************************************************** - * Send ESC motor signals immediately at the end of the control loop - ***************************************************************************/ - for(i=0;i TIP_ANGLE || fabs(state_estimate.pitch) > TIP_ANGLE) + { + feedback_disarm(); + printf("\n TIPOVER DETECTED \n"); + } + + // if not running or not armed, keep the motors in an idle state + if (rc_get_state() != RUNNING || fstate.arm_state == DISARMED) + { + __send_motor_stop_pulse(); + return 0; + } + + // We are about to start marching the individual SISO controllers forward. + // Start by zeroing out the motors signals then add from there. + for (i = 0; i < 8; i++) mot[i] = 0.0; + for (i = 0; i < 6; i++) u[i] = 0.0; + + /*************************************************************************** + * Throttle/Altitude Controller + * + * If transitioning from direct throttle to altitude control, prefill the + * filter with current throttle input to make smooth transition. This is also + * true if taking off for the first time in altitude mode as arm_controller + * sets up last_en_Z_ctrl and last_usr_thr every time controller arms + ***************************************************************************/ + // run altitude controller if enabled + // this needs work... + // we need to: + // find hover thrust and correct from there + // this code does not work a.t.m. + if (setpoint.en_Z_ctrl) + { + if (last_en_Z_ctrl == 0) + { + setpoint.Z = state_estimate.alt_bmp; // set altitude setpoint to current altitude + rc_filter_reset(&D_Z); + tmp = -setpoint.Z_throttle / (cos(state_estimate.roll) * cos(state_estimate.pitch)); + rc_filter_prefill_outputs(&D_Z, tmp); + last_en_Z_ctrl = 1; + } + D_Z.gain = D_Z_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; + tmp = rc_filter_march( + &D_Z, -setpoint.Z + state_estimate.alt_bmp); // altitude is positive but +Z is down + rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); + u[VEC_Z] = tmp / cos(state_estimate.roll) * cos(state_estimate.pitch); + mix_add_input(u[VEC_Z], VEC_Z, mot); + last_en_Z_ctrl = 1; + } + // else use direct throttle + else + { + // compensate for tilt + tmp = setpoint.Z_throttle / (cos(state_estimate.roll) * cos(state_estimate.pitch)); + // printf("throttle: %f\n",tmp); + rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); + u[VEC_Z] = tmp; + mix_add_input(u[VEC_Z], VEC_Z, mot); + } + + /*************************************************************************** + * Roll Pitch Yaw controllers, only run if enabled + ***************************************************************************/ + if (setpoint.en_rpy_ctrl) + { + // Roll + mix_check_saturation(VEC_ROLL, mot, &min, &max); + if (max > MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; + if (min < -MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; + rc_filter_enable_saturation(&D_roll, min, max); + D_roll.gain = D_roll_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; + u[VEC_ROLL] = rc_filter_march(&D_roll, setpoint.roll - state_estimate.roll); + mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); + + // pitch + mix_check_saturation(VEC_PITCH, mot, &min, &max); + if (max > MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; + if (min < -MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; + rc_filter_enable_saturation(&D_pitch, min, max); + D_pitch.gain = D_pitch_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; + u[VEC_PITCH] = rc_filter_march(&D_pitch, setpoint.pitch - state_estimate.pitch); + mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); + + // Yaw + // if throttle stick is down (waiting to take off) keep yaw setpoint at + // current heading, otherwide update by yaw rate + mix_check_saturation(VEC_YAW, mot, &min, &max); + if (max > MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; + if (min < -MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; + rc_filter_enable_saturation(&D_yaw, min, max); + D_yaw.gain = D_yaw_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; + u[VEC_YAW] = rc_filter_march(&D_yaw, setpoint.yaw - state_estimate.yaw); + mix_add_input(u[VEC_YAW], VEC_YAW, mot); + } + // otherwise direct throttle to roll pitch yaw + else + { + // roll + mix_check_saturation(VEC_ROLL, mot, &min, &max); + if (max > MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; + if (min < -MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; + u[VEC_ROLL] = setpoint.roll_throttle; + rc_saturate_double(&u[VEC_ROLL], min, max); + mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); + + // pitch + mix_check_saturation(VEC_PITCH, mot, &min, &max); + if (max > MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; + if (min < -MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; + u[VEC_PITCH] = setpoint.pitch_throttle; + rc_saturate_double(&u[VEC_PITCH], min, max); + mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); + + // YAW + mix_check_saturation(VEC_YAW, mot, &min, &max); + if (max > MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; + if (min < -MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; + u[VEC_YAW] = setpoint.yaw_throttle; + rc_saturate_double(&u[VEC_YAW], min, max); + mix_add_input(u[VEC_YAW], VEC_YAW, mot); + } + + // for 6dof systems, add X and Y + if (setpoint.en_6dof) + { + // X + mix_check_saturation(VEC_X, mot, &min, &max); + if (max > MAX_X_COMPONENT) max = MAX_X_COMPONENT; + if (min < -MAX_X_COMPONENT) min = -MAX_X_COMPONENT; + u[VEC_X] = setpoint.X_throttle; + rc_saturate_double(&u[VEC_X], min, max); + mix_add_input(u[VEC_X], VEC_X, mot); + + // Y + mix_check_saturation(VEC_Y, mot, &min, &max); + if (max > MAX_Y_COMPONENT) max = MAX_Y_COMPONENT; + if (min < -MAX_Y_COMPONENT) min = -MAX_Y_COMPONENT; + u[VEC_Y] = setpoint.Y_throttle; + rc_saturate_double(&u[VEC_Y], min, max); + mix_add_input(u[VEC_Y], VEC_Y, mot); + } + + /*************************************************************************** + * Send ESC motor signals immediately at the end of the control loop + ***************************************************************************/ + for (i = 0; i < settings.num_rotors; i++) + { + rc_saturate_double(&mot[i], 0.0, 1.0); + fstate.m[i] = map_motor_signal(mot[i]); + + // NO NO NO this undoes all the fancy mixing-based saturation + // done above, idle should be done with MAX_THRUST_COMPONENT instead + // rc_saturate_double(&fstate.m[i], MOTOR_IDLE_CMD, 1.0); + + // final saturation just to take care of possible rounding errors + // this should not change the values and is probably excessive + rc_saturate_double(&fstate.m[i], 0.0, 1.0); + + // finally send pulses! + rc_servo_send_esc_pulse_normalized(i + 1, fstate.m[i]); + } + + /*************************************************************************** + * Final cleanup, timing, and indexing + ***************************************************************************/ + // Load control inputs into cstate for viewing by outside threads + for (i = 0; i < 6; i++) fstate.u[i] = u[i]; + // keep track of loops since arming + fstate.loop_index++; + // log us since arming, mostly for the log + fstate.last_step_ns = rc_nanos_since_boot(); + + return 0; } - int feedback_cleanup(void) { - __send_motor_stop_pulse(); - return 0; + __send_motor_stop_pulse(); + return 0; } \ No newline at end of file diff --git a/src/input_manager.c b/src/input_manager.c index 291502d..d5667d8 100644 --- a/src/input_manager.c +++ b/src/input_manager.c @@ -2,45 +2,45 @@ * @file input_manager.c */ +#include +#include // for fabs #include #include -#include -#include // for fabs -#include -#include -#include #include #include +#include +#include +#include #include +#include #include #include -#include #include -user_input_t user_input; // extern variable in input_manager.h +user_input_t user_input; // extern variable in input_manager.h static pthread_t input_manager_thread; -static arm_state_t kill_switch = DISARMED; // raw kill switch on the radio - +static arm_state_t kill_switch = DISARMED; // raw kill switch on the radio /** -* float apply_deadzone(float in, float zone) -* -* Applies a dead zone to an input stick in. in is supposed to range from -1 to 1 -* the dead zone is centered around 0. zone specifies the distance from 0 the -* zone extends. -**/ + * float apply_deadzone(float in, float zone) + * + * Applies a dead zone to an input stick in. in is supposed to range from -1 to 1 + * the dead zone is centered around 0. zone specifies the distance from 0 the + * zone extends. + **/ static double __deadzone(double in, double zone) { - if(zone<=0.0) return in; - if(fabs(in)<=zone) return 0.0; - if(in>0.0) return ((in-zone)/(1.0-zone)); - else return ((in+zone)/(1.0-zone)); + if (zone <= 0.0) return in; + if (fabs(in) <= zone) return 0.0; + if (in > 0.0) + return ((in - zone) / (1.0 - zone)); + else + return ((in + zone) / (1.0 - zone)); } - /** * @brief blocking function that returns after arming sequence is complete * @@ -48,227 +48,252 @@ static double __deadzone(double in, double zone) */ static int __wait_for_arming_sequence() { - // already armed, just return. Should never do this in normal operation though - if(user_input.requested_arm_mode == ARMED) return 0; + // already armed, just return. Should never do this in normal operation though + if (user_input.requested_arm_mode == ARMED) return 0; ARM_SEQUENCE_START: - // wait for feedback controller to have started - while(fstate.initialized==0){ - rc_usleep(100000); - if(rc_get_state()==EXITING) return 0; - } - // wait for level - while(fabs(state_estimate.roll)>ARM_TIP_THRESHOLD||fabs(state_estimate.pitch)>ARM_TIP_THRESHOLD){ - rc_usleep(100000); - if(rc_get_state()==EXITING) return 0; - } - // wait for kill switch to be switched to ARMED - while(kill_switch==DISARMED){ - rc_usleep(100000); - if(rc_get_state()==EXITING) return 0; - } - // wait for throttle up - while(rc_dsm_ch_normalized(settings.dsm_thr_ch)*settings.dsm_thr_pol < 0.9){ - rc_usleep(100000); - if(rc_get_state()==EXITING) return 0; - if(kill_switch==DISARMED) goto ARM_SEQUENCE_START; - } - // wait for throttle down - while(rc_dsm_ch_normalized(settings.dsm_thr_ch)*settings.dsm_thr_pol > -0.9){ - rc_usleep(100000); - if(rc_get_state()==EXITING) return 0; - if(kill_switch==DISARMED) goto ARM_SEQUENCE_START; - } - - // final check of kill switch and level before arming - if(kill_switch==DISARMED) goto ARM_SEQUENCE_START; - if(fabs(state_estimate.roll)>ARM_TIP_THRESHOLD||fabs(state_estimate.pitch)>ARM_TIP_THRESHOLD){ - goto ARM_SEQUENCE_START; - } - return 0; + // wait for feedback controller to have started + while (fstate.initialized == 0) + { + rc_usleep(100000); + if (rc_get_state() == EXITING) return 0; + } + // wait for level + while (fabs(state_estimate.roll) > ARM_TIP_THRESHOLD || + fabs(state_estimate.pitch) > ARM_TIP_THRESHOLD) + { + rc_usleep(100000); + if (rc_get_state() == EXITING) return 0; + } + // wait for kill switch to be switched to ARMED + while (kill_switch == DISARMED) + { + rc_usleep(100000); + if (rc_get_state() == EXITING) return 0; + } + // wait for throttle up + while (rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol < 0.9) + { + rc_usleep(100000); + if (rc_get_state() == EXITING) return 0; + if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; + } + // wait for throttle down + while (rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol > -0.9) + { + rc_usleep(100000); + if (rc_get_state() == EXITING) return 0; + if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; + } + + // final check of kill switch and level before arming + if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; + if (fabs(state_estimate.roll) > ARM_TIP_THRESHOLD || + fabs(state_estimate.pitch) > ARM_TIP_THRESHOLD) + { + goto ARM_SEQUENCE_START; + } + return 0; } - void new_dsm_data_callback() { - double new_thr, new_roll, new_pitch, new_yaw, new_mode, new_kill; - - // Read normalized (+-1) inputs from RC radio stick and multiply by - // polarity setting so positive stick means positive setpoint - new_thr = rc_dsm_ch_normalized(settings.dsm_thr_ch)*settings.dsm_thr_pol; - new_roll = rc_dsm_ch_normalized(settings.dsm_roll_ch)*settings.dsm_roll_pol; - new_pitch = rc_dsm_ch_normalized(settings.dsm_pitch_ch)*settings.dsm_pitch_pol; - new_yaw = __deadzone(rc_dsm_ch_normalized(settings.dsm_yaw_ch)*settings.dsm_yaw_pol, YAW_DEADZONE); - new_mode = rc_dsm_ch_normalized(settings.dsm_mode_ch)*settings.dsm_mode_pol; - - - // kill mode behaviors - switch(settings.dsm_kill_mode){ - case DSM_KILL_DEDICATED_SWITCH: - new_kill = rc_dsm_ch_normalized(settings.dsm_kill_ch)*settings.dsm_kill_pol; - // determine the kill state - if(new_kill<=0.1){ - kill_switch = DISARMED; - user_input.requested_arm_mode=DISARMED; - } - else{ - kill_switch = ARMED; - } - break; - - case DSM_KILL_NEGATIVE_THROTTLE: - if(new_thr<=-1.1){ - kill_switch = DISARMED; - user_input.requested_arm_mode=DISARMED; - } - else kill_switch = ARMED; - break; - - default: - fprintf(stderr, "ERROR in input manager, unhandled settings.dsm_kill_mode\n"); - return; - } - - // saturate the sticks to avoid possible erratic behavior - // throttle can drop below -1 so extend the range for thr - rc_saturate_double(&new_thr, -1.0, 1.0); - rc_saturate_double(&new_roll, -1.0, 1.0); - rc_saturate_double(&new_pitch, -1.0, 1.0); - rc_saturate_double(&new_yaw, -1.0, 1.0); - - // pick flight mode - switch(settings.num_dsm_modes){ - case 1: - user_input.flight_mode = settings.flight_mode_1; - break; - case 2: - // switch will either range from -1 to 1 or 0 to 1. - // in either case it's safe to use +0.5 as the cutoff - if(new_mode>0.5) user_input.flight_mode = settings.flight_mode_2; - else user_input.flight_mode = settings.flight_mode_1; - break; - case 3: - // 3-position switch will have the positions -1, 0, 1 when - // calibrated correctly. checking +- 0.5 is a safe cutoff - if(new_mode>0.5) user_input.flight_mode = settings.flight_mode_3; - else if(new_mode<-0.5) user_input.flight_mode = settings.flight_mode_1; - else user_input.flight_mode = settings.flight_mode_2; - break; - default: - fprintf(stderr,"ERROR, in input_manager, num_dsm_modes must be 1,2 or 3\n"); - fprintf(stderr,"selecting flight mode 1\n"); - user_input.flight_mode = settings.flight_mode_1; - break; - } - - // fill in sticks - if(user_input.requested_arm_mode==ARMED){ - user_input.thr_stick = new_thr; - user_input.roll_stick = new_roll; - user_input.pitch_stick = new_pitch; - user_input.yaw_stick = new_yaw; - user_input.requested_arm_mode = kill_switch; - } - else{ - // during arming sequence keep sticks zeroed - user_input.thr_stick = 0.0; - user_input.roll_stick = 0.0; - user_input.pitch_stick = 0.0; - user_input.yaw_stick = 0.0; - } - - if(user_input.input_active==0){ - user_input.input_active=1; // flag that connection has come back online - printf("DSM CONNECTION ESTABLISHED\n"); - } - return; - + double new_thr, new_roll, new_pitch, new_yaw, new_mode, new_kill; + + // Read normalized (+-1) inputs from RC radio stick and multiply by + // polarity setting so positive stick means positive setpoint + new_thr = rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol; + new_roll = rc_dsm_ch_normalized(settings.dsm_roll_ch) * settings.dsm_roll_pol; + new_pitch = rc_dsm_ch_normalized(settings.dsm_pitch_ch) * settings.dsm_pitch_pol; + new_yaw = + __deadzone(rc_dsm_ch_normalized(settings.dsm_yaw_ch) * settings.dsm_yaw_pol, YAW_DEADZONE); + new_mode = rc_dsm_ch_normalized(settings.dsm_mode_ch) * settings.dsm_mode_pol; + + // kill mode behaviors + switch (settings.dsm_kill_mode) + { + case DSM_KILL_DEDICATED_SWITCH: + new_kill = rc_dsm_ch_normalized(settings.dsm_kill_ch) * settings.dsm_kill_pol; + // determine the kill state + if (new_kill <= 0.1) + { + kill_switch = DISARMED; + user_input.requested_arm_mode = DISARMED; + } + else + { + kill_switch = ARMED; + } + break; + + case DSM_KILL_NEGATIVE_THROTTLE: + if (new_thr <= -1.1) + { + kill_switch = DISARMED; + user_input.requested_arm_mode = DISARMED; + } + else + kill_switch = ARMED; + break; + + default: + fprintf(stderr, "ERROR in input manager, unhandled settings.dsm_kill_mode\n"); + return; + } + + // saturate the sticks to avoid possible erratic behavior + // throttle can drop below -1 so extend the range for thr + rc_saturate_double(&new_thr, -1.0, 1.0); + rc_saturate_double(&new_roll, -1.0, 1.0); + rc_saturate_double(&new_pitch, -1.0, 1.0); + rc_saturate_double(&new_yaw, -1.0, 1.0); + + // pick flight mode + switch (settings.num_dsm_modes) + { + case 1: + user_input.flight_mode = settings.flight_mode_1; + break; + case 2: + // switch will either range from -1 to 1 or 0 to 1. + // in either case it's safe to use +0.5 as the cutoff + if (new_mode > 0.5) + user_input.flight_mode = settings.flight_mode_2; + else + user_input.flight_mode = settings.flight_mode_1; + break; + case 3: + // 3-position switch will have the positions -1, 0, 1 when + // calibrated correctly. checking +- 0.5 is a safe cutoff + if (new_mode > 0.5) + user_input.flight_mode = settings.flight_mode_3; + else if (new_mode < -0.5) + user_input.flight_mode = settings.flight_mode_1; + else + user_input.flight_mode = settings.flight_mode_2; + break; + default: + fprintf(stderr, "ERROR, in input_manager, num_dsm_modes must be 1,2 or 3\n"); + fprintf(stderr, "selecting flight mode 1\n"); + user_input.flight_mode = settings.flight_mode_1; + break; + } + + // fill in sticks + if (user_input.requested_arm_mode == ARMED) + { + user_input.thr_stick = new_thr; + user_input.roll_stick = new_roll; + user_input.pitch_stick = new_pitch; + user_input.yaw_stick = new_yaw; + user_input.requested_arm_mode = kill_switch; + } + else + { + // during arming sequence keep sticks zeroed + user_input.thr_stick = 0.0; + user_input.roll_stick = 0.0; + user_input.pitch_stick = 0.0; + user_input.yaw_stick = 0.0; + } + + if (user_input.input_active == 0) + { + user_input.input_active = 1; // flag that connection has come back online + printf("DSM CONNECTION ESTABLISHED\n"); + } + return; } - void dsm_disconnect_callback(void) { - user_input.thr_stick = 0.0; - user_input.roll_stick = 0.0; - user_input.pitch_stick = 0.0; - user_input.yaw_stick = 0.0; - user_input.input_active = 0; - kill_switch = DISARMED; - user_input.requested_arm_mode=DISARMED; - fprintf(stderr, "LOST DSM CONNECTION\n"); + user_input.thr_stick = 0.0; + user_input.roll_stick = 0.0; + user_input.pitch_stick = 0.0; + user_input.yaw_stick = 0.0; + user_input.input_active = 0; + kill_switch = DISARMED; + user_input.requested_arm_mode = DISARMED; + fprintf(stderr, "LOST DSM CONNECTION\n"); } - void* input_manager(void* ptr) { - user_input.initialized = 1; - // wait for first packet - while(rc_get_state()!=EXITING){ - if(user_input.input_active) break; - rc_usleep(1000000/INPUT_MANAGER_HZ); - } - - // not much to do since the DSM callbacks to most of it. Later some - // logic to handle other inputs such as mavlink/bluetooth/wifi - while(rc_get_state()!=EXITING){ - // if the core got disarmed, wait for arming sequence - if(user_input.requested_arm_mode == DISARMED){ - __wait_for_arming_sequence(); - // user may have pressed the pause button or shut down while waiting - // check before continuing - if(rc_get_state()!=RUNNING) continue; - else{ - user_input.requested_arm_mode=ARMED; - //printf("\n\nDSM ARM REQUEST\n\n"); - } - } - // wait - rc_usleep(1000000/INPUT_MANAGER_HZ); - } - return NULL; + user_input.initialized = 1; + // wait for first packet + while (rc_get_state() != EXITING) + { + if (user_input.input_active) break; + rc_usleep(1000000 / INPUT_MANAGER_HZ); + } + + // not much to do since the DSM callbacks to most of it. Later some + // logic to handle other inputs such as mavlink/bluetooth/wifi + while (rc_get_state() != EXITING) + { + // if the core got disarmed, wait for arming sequence + if (user_input.requested_arm_mode == DISARMED) + { + __wait_for_arming_sequence(); + // user may have pressed the pause button or shut down while waiting + // check before continuing + if (rc_get_state() != RUNNING) + continue; + else + { + user_input.requested_arm_mode = ARMED; + // printf("\n\nDSM ARM REQUEST\n\n"); + } + } + // wait + rc_usleep(1000000 / INPUT_MANAGER_HZ); + } + return NULL; } - - int input_manager_init() { - user_input.initialized = 0; - int i; - // start dsm hardware - if(rc_dsm_init()==-1){ - fprintf(stderr, "ERROR in input_manager_init, failed to initialize dsm\n"); - return -1; - } - rc_dsm_set_disconnect_callback(dsm_disconnect_callback); - rc_dsm_set_callback(new_dsm_data_callback); - // start thread - if(rc_pthread_create(&input_manager_thread, &input_manager, NULL, - SCHED_FIFO, INPUT_MANAGER_PRI)==-1){ - fprintf(stderr, "ERROR in input_manager_init, failed to start thread\n"); - return -1; - } - // wait for thread to start - for(i=0;i<50;i++){ - if(user_input.initialized) return 0; - rc_usleep(50000); - } - fprintf(stderr, "ERROR in input_manager_init, timeout waiting for thread to start\n"); - return -1; + user_input.initialized = 0; + int i; + // start dsm hardware + if (rc_dsm_init() == -1) + { + fprintf(stderr, "ERROR in input_manager_init, failed to initialize dsm\n"); + return -1; + } + rc_dsm_set_disconnect_callback(dsm_disconnect_callback); + rc_dsm_set_callback(new_dsm_data_callback); + // start thread + if (rc_pthread_create( + &input_manager_thread, &input_manager, NULL, SCHED_FIFO, INPUT_MANAGER_PRI) == -1) + { + fprintf(stderr, "ERROR in input_manager_init, failed to start thread\n"); + return -1; + } + // wait for thread to start + for (i = 0; i < 50; i++) + { + if (user_input.initialized) return 0; + rc_usleep(50000); + } + fprintf(stderr, "ERROR in input_manager_init, timeout waiting for thread to start\n"); + return -1; } - int input_manager_cleanup() { - if(user_input.initialized==0){ - fprintf(stderr, "WARNING in input_manager_cleanup, was never initialized\n"); - return -1; - } - // wait for the thread to exit - if(rc_pthread_timed_join(input_manager_thread, NULL, INPUT_MANAGER_TOUT)==1){ - fprintf(stderr,"WARNING: in input_manager_cleanup, thread join timeout\n"); - return -1; - } - // stop dsm - rc_dsm_cleanup(); - return 0; + if (user_input.initialized == 0) + { + fprintf(stderr, "WARNING in input_manager_cleanup, was never initialized\n"); + return -1; + } + // wait for the thread to exit + if (rc_pthread_timed_join(input_manager_thread, NULL, INPUT_MANAGER_TOUT) == 1) + { + fprintf(stderr, "WARNING: in input_manager_cleanup, thread join timeout\n"); + return -1; + } + // stop dsm + rc_dsm_cleanup(); + return 0; } diff --git a/src/log_manager.c b/src/log_manager.c index b360d56..a2a8fb1 100644 --- a/src/log_manager.c +++ b/src/log_manager.c @@ -2,357 +2,341 @@ * @file log_manager.c */ - -#include -#include -#include -#include -#include #include +#include +#include #include - +#include +#include +#include // to allow printf macros for multi-architecture portability #define __STDC_FORMAT_MACROS #include - +#include #include #include -#include -#include -#include +#include #include -#include +#include #include -#include +#include #include +#include +#define MAX_LOG_FILES 500 +#define BUF_LEN 50 -#define MAX_LOG_FILES 500 -#define BUF_LEN 50 - -static uint64_t num_entries; // number of entries logged so far -static int buffer_pos; // position in current buffer -static int current_buf; // 0 or 1 to indicate which buffer is being filled -static int needs_writing; // flag set to 1 if a buffer is full -static FILE* fd; // file descriptor for the log file +static uint64_t num_entries; // number of entries logged so far +static int buffer_pos; // position in current buffer +static int current_buf; // 0 or 1 to indicate which buffer is being filled +static int needs_writing; // flag set to 1 if a buffer is full +static FILE* fd; // file descriptor for the log file // array of two buffers so one can fill while writing the other to file static log_entry_t buffer[2][BUF_LEN]; // background thread and running flag static pthread_t pthread; -static int logging_enabled; // set to 0 to exit the write_thread - +static int logging_enabled; // set to 0 to exit the write_thread static int __write_header(FILE* fd) { - // always print loop index - fprintf(fd, "loop_index,last_step_ns"); - - if(settings.log_sensors){ - fprintf(fd, ",v_batt,alt_bmp_raw,gyro_roll,gyro_pitch,gyro_yaw,accel_X,accel_Y,accel_Z"); - } - - if(settings.log_state){ - fprintf(fd, ",roll,pitch,yaw,X,Y,Z,Xdot,Ydot,Zdot"); - } - - if(settings.log_setpoint){ - fprintf(fd, ",sp_roll,sp_pitch,sp_yaw,sp_X,sp_Y,sp_Z,sp_Xdot,sp_Ydot,sp_Zdot"); - } - - if(settings.log_control_u){ - fprintf(fd, ",u_roll,u_pitch,u_yaw,u_X,u_Y,u_Z"); - } - - if(settings.log_motor_signals && settings.num_rotors==8){ - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6,mot_7,mot_8"); - } - if(settings.log_motor_signals && settings.num_rotors==6){ - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6"); - } - if(settings.log_motor_signals && settings.num_rotors==4){ - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4"); - } - - fprintf(fd, "\n"); - return 0; - - + // always print loop index + fprintf(fd, "loop_index,last_step_ns"); + + if (settings.log_sensors) + { + fprintf(fd, ",v_batt,alt_bmp_raw,gyro_roll,gyro_pitch,gyro_yaw,accel_X,accel_Y,accel_Z"); + } + + if (settings.log_state) + { + fprintf(fd, ",roll,pitch,yaw,X,Y,Z,Xdot,Ydot,Zdot"); + } + + if (settings.log_setpoint) + { + fprintf(fd, ",sp_roll,sp_pitch,sp_yaw,sp_X,sp_Y,sp_Z,sp_Xdot,sp_Ydot,sp_Zdot"); + } + + if (settings.log_control_u) + { + fprintf(fd, ",u_roll,u_pitch,u_yaw,u_X,u_Y,u_Z"); + } + + if (settings.log_motor_signals && settings.num_rotors == 8) + { + fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6,mot_7,mot_8"); + } + if (settings.log_motor_signals && settings.num_rotors == 6) + { + fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6"); + } + if (settings.log_motor_signals && settings.num_rotors == 4) + { + fprintf(fd, ",mot_1,mot_2,mot_3,mot_4"); + } + + fprintf(fd, "\n"); + return 0; } - static int __write_log_entry(FILE* fd, log_entry_t e) { - // always print loop index - fprintf(fd, "%" PRIu64 ",%" PRIu64, e.loop_index, e.last_step_ns); - - if(settings.log_sensors){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F",\ - e.v_batt,\ - e.alt_bmp_raw,\ - e.gyro_roll,\ - e.gyro_pitch,\ - e.gyro_yaw,\ - e.accel_X,\ - e.accel_Y,\ - e.accel_Z); - } - - if(settings.log_state){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F",\ - e.roll,\ - e.pitch,\ - e.yaw,\ - e.X,\ - e.Y,\ - e.Z,\ - e.Xdot,\ - e.Ydot,\ - e.Zdot); - } - - if(settings.log_setpoint){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F",\ - e.sp_roll,\ - e.sp_pitch,\ - e.sp_yaw,\ - e.sp_X,\ - e.sp_Y,\ - e.sp_Z,\ - e.sp_Xdot,\ - e.sp_Ydot,\ - e.sp_Zdot); - } - - if(settings.log_control_u){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F",\ - e.u_roll,\ - e.u_pitch,\ - e.u_yaw,\ - e.u_X,\ - e.u_Y,\ - e.u_Z); - } - - if(settings.log_motor_signals && settings.num_rotors==8){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1,\ - e.mot_2,\ - e.mot_3,\ - e.mot_4,\ - e.mot_5,\ - e.mot_6,\ - e.mot_7,\ - e.mot_8); - } - if(settings.log_motor_signals && settings.num_rotors==6){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1,\ - e.mot_2,\ - e.mot_3,\ - e.mot_4,\ - e.mot_5,\ - e.mot_6); - } - if(settings.log_motor_signals && settings.num_rotors==4){ - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F", e.mot_1,\ - e.mot_2,\ - e.mot_3,\ - e.mot_4); - } - - fprintf(fd, "\n"); - return 0; + // always print loop index + fprintf(fd, "%" PRIu64 ",%" PRIu64, e.loop_index, e.last_step_ns); + + if (settings.log_sensors) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.v_batt, e.alt_bmp_raw, + e.gyro_roll, e.gyro_pitch, e.gyro_yaw, e.accel_X, e.accel_Y, e.accel_Z); + } + + if (settings.log_state) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.roll, e.pitch, e.yaw, e.X, + e.Y, e.Z, e.Xdot, e.Ydot, e.Zdot); + } + + if (settings.log_setpoint) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.sp_roll, e.sp_pitch, + e.sp_yaw, e.sp_X, e.sp_Y, e.sp_Z, e.sp_Xdot, e.sp_Ydot, e.sp_Zdot); + } + + if (settings.log_control_u) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.u_roll, e.u_pitch, e.u_yaw, + e.u_X, e.u_Y, e.u_Z); + } + + if (settings.log_motor_signals && settings.num_rotors == 8) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, + e.mot_5, e.mot_6, e.mot_7, e.mot_8); + } + if (settings.log_motor_signals && settings.num_rotors == 6) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, e.mot_5, + e.mot_6); + } + if (settings.log_motor_signals && settings.num_rotors == 4) + { + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4); + } + + fprintf(fd, "\n"); + return 0; } - -static void* __log_manager_func(__attribute__ ((unused)) void* ptr) +static void* __log_manager_func(__attribute__((unused)) void* ptr) { - int i, buf_to_write; - // while logging enabled and not exiting, write full buffers to disk - while(rc_get_state()!=EXITING && logging_enabled){ - if(needs_writing){ - // buffer to be written is opposite of one currently being filled - if(current_buf==0) buf_to_write=1; - else buf_to_write=0; - // write the full buffer to disk; - for(i=0;i= BUF_LEN){ - fprintf(stderr,"WARNING: logging buffer full, skipping log entry\n"); - return -1; - } - // add to buffer and increment counters - buffer[current_buf][buffer_pos] = __construct_new_entry(); - buffer_pos++; - num_entries++; - // check if we've filled a buffer - if(buffer_pos >= BUF_LEN){ - buffer_pos = 0; // reset buffer position to 0 - needs_writing = 1; // flag the writer to dump to disk - // swap buffers - if(current_buf==0) current_buf=1; - else current_buf=0; - } - return 0; + if (!logging_enabled) + { + fprintf(stderr, "ERROR: trying to log entry while logger isn't running\n"); + return -1; + } + if (needs_writing && buffer_pos >= BUF_LEN) + { + fprintf(stderr, "WARNING: logging buffer full, skipping log entry\n"); + return -1; + } + // add to buffer and increment counters + buffer[current_buf][buffer_pos] = __construct_new_entry(); + buffer_pos++; + num_entries++; + // check if we've filled a buffer + if (buffer_pos >= BUF_LEN) + { + buffer_pos = 0; // reset buffer position to 0 + needs_writing = 1; // flag the writer to dump to disk + // swap buffers + if (current_buf == 0) + current_buf = 1; + else + current_buf = 0; + } + return 0; } int log_manager_cleanup() { - // just return if not logging - if(logging_enabled==0) return 0; - - // disable logging so the thread can stop and start multiple times - // thread also exits on rc_get_state()==EXITING - logging_enabled=0; - int ret = rc_pthread_timed_join(pthread,NULL,LOG_MANAGER_TOUT); - if(ret==1) fprintf(stderr,"WARNING: log_manager_thread exit timeout\n"); - else if(ret==-1) fprintf(stderr,"ERROR: failed to join log_manager thread\n"); - return ret; + // just return if not logging + if (logging_enabled == 0) return 0; + + // disable logging so the thread can stop and start multiple times + // thread also exits on rc_get_state()==EXITING + logging_enabled = 0; + int ret = rc_pthread_timed_join(pthread, NULL, LOG_MANAGER_TOUT); + if (ret == 1) + fprintf(stderr, "WARNING: log_manager_thread exit timeout\n"); + else if (ret == -1) + fprintf(stderr, "ERROR: failed to join log_manager thread\n"); + return ret; } diff --git a/src/main.c b/src/main.c index 169f983..a462e56 100644 --- a/src/main.c +++ b/src/main.c @@ -1,52 +1,50 @@ /** -* @file main.c -* -* see README.txt for description and use -**/ + * @file main.c + * + * see README.txt for description and use + **/ +#include #include #include -#include -#include #include -#include -#include -#include #include #include +#include +#include #include +#include +#include +#include #include -#include -#include // contains extern settings variable -#include -#include #include -#include -#include #include +#include #include +#include +#include // contains extern settings variable +#include +#include -#define FAIL(str) \ -fprintf(stderr, str); \ -rc_led_set(RC_LED_GREEN,0); \ -rc_led_blink(RC_LED_RED,8.0,2.0); \ -return -1; +#define FAIL(str) \ + fprintf(stderr, str); \ + rc_led_set(RC_LED_GREEN, 0); \ + rc_led_blink(RC_LED_RED, 8.0, 2.0); \ + return -1; void print_usage() { - printf("\n"); - printf(" Options\n"); - printf(" -s {settings file} Specify settings file to use\n"); - printf(" -h Print this help message\n"); - printf("\n"); - printf("Some example settings files are included with the\n"); - printf("source code. You must specify the location of one of these\n"); - printf("files or ideally the location of your own settings file.\n"); - printf("\n"); - - + printf("\n"); + printf(" Options\n"); + printf(" -s {settings file} Specify settings file to use\n"); + printf(" -h Print this help message\n"); + printf("\n"); + printf("Some example settings files are included with the\n"); + printf("source code. You must specify the location of one of these\n"); + printf("files or ideally the location of your own settings file.\n"); + printf("\n"); } /** @@ -54,41 +52,47 @@ void print_usage() */ int __rc_dsm_is_calibrated() { - if(!access("/var/lib/robotcontrol/dsm.cal", F_OK)) return 1; - else return 0; + if (!access("/var/lib/robotcontrol/dsm.cal", F_OK)) + return 1; + else + return 0; } /** -* If the user holds the pause button for 2 seconds, set state to exiting which -* triggers the rest of the program to exit cleanly. -*/ + * If the user holds the pause button for 2 seconds, set state to exiting which + * triggers the rest of the program to exit cleanly. + */ void on_pause_press() { - int i=0; - const int quit_check_us = 100000; - const int samples = 2000000/quit_check_us; - - // toggle betewen paused and running modes - if(rc_get_state()==RUNNING){ - rc_set_state(PAUSED); - printf("PAUSED\n"); - } - else if(rc_get_state()==PAUSED){ - rc_set_state(RUNNING); - printf("RUNNING\n"); - } - fflush(stdout); - - // now keep checking to see if the button is still held down - for(i=0;i -#include +#include #include +#include #include -#include #include #include +#include -#define LOCALHOST_IP "127.0.0.1" -#define DEFAULT_SYS_ID 1 - - +#define LOCALHOST_IP "127.0.0.1" +#define DEFAULT_SYS_ID 1 static void __callback_func_mocap(void) { - int i; - mavlink_att_pos_mocap_t data; - - if(rc_mav_get_att_pos_mocap(&data)<0){ - fprintf(stderr, "ERROR in mavlink manager, problem fetching att_pos_mocal packet\n"); - return; - } - - // check if position is 0 0 0 which indicates mocap system is alive but - // has lost visual contact on the object - if(fabs(data.x)<0.0001 && fabs(data.y)<0.0001 && fabs(data.z)<0.0001){ - if(state_estimate.mocap_running==1){ - state_estimate.mocap_running=0; - if(settings.warnings_en){ - fprintf(stderr,"WARNING, MOCAP LOST VISUAL\n"); - } - } - else{ - state_estimate.is_active=0; - } - return; - } - - // copy data - for(i=0;i<4;i++) state_estimate.quat_mocap[i]=(double)data.q[i]; - // normalize quaternion because we don't trust the mocap system - rc_quaternion_norm_array(state_estimate.quat_mocap); - // calculate tait bryan angles too - rc_quaternion_to_tb_array(state_estimate.quat_mocap, state_estimate.tb_mocap); - // position - state_estimate.pos_mocap[0]=(double)data.x; - state_estimate.pos_mocap[1]=(double)data.y; - state_estimate.pos_mocap[2]=(double)data.z; - // mark timestamp - state_estimate.mocap_timestamp_ns=rc_nanos_since_boot(); - state_estimate.mocap_running=1; - - return; + int i; + mavlink_att_pos_mocap_t data; + + if (rc_mav_get_att_pos_mocap(&data) < 0) + { + fprintf(stderr, "ERROR in mavlink manager, problem fetching att_pos_mocal packet\n"); + return; + } + + // check if position is 0 0 0 which indicates mocap system is alive but + // has lost visual contact on the object + if (fabs(data.x) < 0.0001 && fabs(data.y) < 0.0001 && fabs(data.z) < 0.0001) + { + if (state_estimate.mocap_running == 1) + { + state_estimate.mocap_running = 0; + if (settings.warnings_en) + { + fprintf(stderr, "WARNING, MOCAP LOST VISUAL\n"); + } + } + else + { + state_estimate.is_active = 0; + } + return; + } + + // copy data + for (i = 0; i < 4; i++) state_estimate.quat_mocap[i] = (double)data.q[i]; + // normalize quaternion because we don't trust the mocap system + rc_quaternion_norm_array(state_estimate.quat_mocap); + // calculate tait bryan angles too + rc_quaternion_to_tb_array(state_estimate.quat_mocap, state_estimate.tb_mocap); + // position + state_estimate.pos_mocap[0] = (double)data.x; + state_estimate.pos_mocap[1] = (double)data.y; + state_estimate.pos_mocap[2] = (double)data.z; + // mark timestamp + state_estimate.mocap_timestamp_ns = rc_nanos_since_boot(); + state_estimate.mocap_running = 1; + + return; } - - int mavlink_manager_init(void) { - // set default options before checking options - const char* dest_ip=LOCALHOST_IP; - uint8_t my_sys_id=DEFAULT_SYS_ID; - uint16_t port=RC_MAV_DEFAULT_UDP_PORT; - - // initialize the UDP port and listening thread with the rc_mav lib - if(rc_mav_init(settings.my_sys_id, settings.dest_ip, \ - settings.mav_port, RC_MAV_DEFAULT_CONNECTION_TIMEOUT_US) < 0) return -1; - - // set the mocap callback to record position - rc_mav_set_callback(MAVLINK_MSG_ID_ATT_POS_MOCAP, __callback_func_mocap); - return 0; + // set default options before checking options + const char* dest_ip = LOCALHOST_IP; + uint8_t my_sys_id = DEFAULT_SYS_ID; + uint16_t port = RC_MAV_DEFAULT_UDP_PORT; + + // initialize the UDP port and listening thread with the rc_mav lib + if (rc_mav_init(settings.my_sys_id, settings.dest_ip, settings.mav_port, + RC_MAV_DEFAULT_CONNECTION_TIMEOUT_US) < 0) + return -1; + + // set the mocap callback to record position + rc_mav_set_callback(MAVLINK_MSG_ID_ATT_POS_MOCAP, __callback_func_mocap); + return 0; } int cleanup_mavlink_manager(void) { - return rc_mav_cleanup(); + return rc_mav_cleanup(); } \ No newline at end of file diff --git a/src/mix.c b/src/mix.c index 2446280..f4f9710 100644 --- a/src/mix.c +++ b/src/mix.c @@ -2,12 +2,12 @@ * @file mixing_matrix.c */ +#include // for DBL_MAX +#include #include #include -#include // for DBL_MAX -#include - +// clang-format off /** * Most popular: 4-rotor X layout like DJI Phantom and 3DR Iris * top view: @@ -18,11 +18,11 @@ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-4 */ -static double mix_4x[][6] = { \ -{0.0, 0.0, -1.0, -0.5, 0.5, 0.5},\ -{0.0, 0.0, -1.0, -0.5, -0.5, -0.5},\ -{0.0, 0.0, -1.0, 0.5, -0.5, 0.5},\ -{0.0, 0.0, -1.0, 0.5, 0.5, -0.5}}; +static double mix_4x[][6] = { + {0.0, 0.0, -1.0, -0.5, 0.5, 0.5}, + {0.0, 0.0, -1.0, -0.5, -0.5, -0.5}, + {0.0, 0.0, -1.0, 0.5, -0.5, 0.5}, + {0.0, 0.0, -1.0, 0.5, 0.5, -0.5}}; /** * less popular: 4-rotor + layout @@ -34,11 +34,11 @@ static double mix_4x[][6] = { \ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-4 */ -static double mix_4plus[][6] = { \ -{0.0, 0.0, -1.0, 0.0, 0.5, 0.5},\ -{0.0, 0.0, -1.0, -0.5, 0.0, -0.5},\ -{0.0, 0.0, -1.0, 0.0, -0.5, 0.5},\ -{0.0, 0.0, -1.0, 0.5, 0.0, -0.5}}; +static double mix_4plus[][6] = { + {0.0, 0.0, -1.0, 0.0, 0.5, 0.5}, + {0.0, 0.0, -1.0, -0.5, 0.0, -0.5}, + {0.0, 0.0, -1.0, 0.0, -0.5, 0.5}, + {0.0, 0.0, -1.0, 0.5, 0.0, -0.5}}; /* * 6X like DJI S800 @@ -50,13 +50,13 @@ static double mix_4plus[][6] = { \ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-6 */ -static double mix_6x[][6] = { \ -{0.0, 0.0, -1.0, -0.25, 0.5, 0.5},\ -{0.0, 0.0, -1.0, -0.50, 0.0, -0.5},\ -{0.0, 0.0, -1.0, -0.25, -0.5, 0.5},\ -{0.0, 0.0, -1.0, 0.25, -0.5, -0.5},\ -{0.0, 0.0, -1.0, 0.50, 0.0, 0.5},\ -{0.0, 0.0, -1.0, 0.25, 0.5, -0.5}}; +static double mix_6x[][6] = { + {0.0, 0.0, -1.0, -0.25, 0.5, 0.5}, + {0.0, 0.0, -1.0, -0.50, 0.0, -0.5}, + {0.0, 0.0, -1.0, -0.25, -0.5, 0.5}, + {0.0, 0.0, -1.0, 0.25, -0.5, -0.5}, + {0.0, 0.0, -1.0, 0.50, 0.0, 0.5}, + {0.0, 0.0, -1.0, 0.25, 0.5, -0.5}}; /** * 8X like DJI S1000 @@ -69,15 +69,15 @@ static double mix_6x[][6] = { \ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-8 */ -static double mix_8x[][6] = { \ -{0.0, 0.0, -1.0, -0.21, 0.50, 0.5},\ -{0.0, 0.0, -1.0, -0.50, 0.21, -0.5},\ -{0.0, 0.0, -1.0, -0.50, -0.21, 0.5},\ -{0.0, 0.0, -1.0, -0.21, -0.50, -0.5},\ -{0.0, 0.0, -1.0, 0.21, -0.50, 0.5},\ -{0.0, 0.0, -1.0, 0.50, -0.21, -0.5},\ -{0.0, 0.0, -1.0, 0.50, 0.21, 0.5},\ -{0.0, 0.0, -1.0, 0.21, 0.50, -0.5}}; +static double mix_8x[][6] = { + {0.0, 0.0, -1.0, -0.21, 0.50, 0.5}, + {0.0, 0.0, -1.0, -0.50, 0.21, -0.5}, + {0.0, 0.0, -1.0, -0.50, -0.21, 0.5}, + {0.0, 0.0, -1.0, -0.21, -0.50, -0.5}, + {0.0, 0.0, -1.0, 0.21, -0.50, 0.5}, + {0.0, 0.0, -1.0, 0.50, -0.21, -0.5}, + {0.0, 0.0, -1.0, 0.50, 0.21, 0.5}, + {0.0, 0.0, -1.0, 0.21, 0.50, -0.5}}; /** * 6D0F control for rotorbits platform @@ -89,14 +89,13 @@ static double mix_8x[][6] = { \ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-6 */ -static double mix_6dof_rotorbits[][6] = { \ -{-0.2736, 0.3638, -1.0000, -0.2293, 0.3921, 0.3443},\ -{ 0.6362, 0.0186, -1.0000, -0.3638, -0.0297, -0.3638},\ -{-0.3382, -0.3533, -1.0000, -0.3320, -0.3638, 0.3546},\ -{-0.3382, 0.3533, -1.0000, 0.3320, -0.3638, -0.3546},\ -{ 0.6362, -0.0186, -1.0000, 0.3638, -0.0297, 0.3638},\ -{-0.2736, -0.3638, -1.0000, 0.2293, 0.3921, -0.3443}}; - +static double mix_6dof_rotorbits[][6] = { + {-0.2736, 0.3638, -1.0000, -0.2293, 0.3921, 0.3443}, + {0.6362, 0.0186, -1.0000, -0.3638, -0.0297, -0.3638}, + {-0.3382, -0.3533, -1.0000, -0.3320, -0.3638, 0.3546}, + {-0.3382, 0.3533, -1.0000, 0.3320, -0.3638, -0.3546}, + {0.6362, -0.0186, -1.0000, 0.3638, -0.0297, 0.3638}, + {-0.2736, -0.3638, -1.0000, 0.2293, 0.3921, -0.3443}}; /** * 6D0F control for 5-inch nylon monocoque @@ -108,190 +107,208 @@ static double mix_6dof_rotorbits[][6] = { \ * columns: X Y Z Roll Pitch Yaw * rows: motors 1-6 */ -static double mix_6dof_5inch_monocoque[][6] = { \ -{-0.2296, 0.2296, -1.0000, -0.2289, 0.2296, 0.2221},\ -{ 0.4742, 0.0000, -1.0000, -0.2296, -0.0000, -0.2296},\ -{-0.2296, -0.2296, -1.0000, -0.2289, -0.2296, 0.2221},\ -{-0.2296, 0.2296, -1.0000, 0.2289, -0.2296, -0.2221},\ -{ 0.4742, -0.0000, -1.0000, 0.2296, -0.0000, 0.2296},\ -{-0.2296, -0.2296, -1.0000, 0.2289, 0.2296, -0.2221}}; +static double mix_6dof_5inch_monocoque[][6] = { + {-0.2296, 0.2296, -1.0000, -0.2289, 0.2296, 0.2221}, + {0.4742, 0.0000, -1.0000, -0.2296, -0.0000, -0.2296}, + {-0.2296, -0.2296, -1.0000, -0.2289, -0.2296, 0.2221}, + {-0.2296, 0.2296, -1.0000, 0.2289, -0.2296, -0.2221}, + {0.4742, -0.0000, -1.0000, 0.2296, -0.0000, 0.2296}, + {-0.2296, -0.2296, -1.0000, 0.2289, 0.2296, -0.2221}}; +// clang-format off static double (*mix_matrix)[6]; static int initialized; static int rotors; static int dof; - int mix_init(rotor_layout_t layout) { - switch(layout){ - case LAYOUT_4X: - rotors = 4; - dof = 4; - mix_matrix = mix_4x; - break; - case LAYOUT_4PLUS: - rotors = 4; - dof = 4; - mix_matrix = mix_4plus; - break; - case LAYOUT_6X: - rotors = 6; - dof = 4; - mix_matrix = mix_6x; - break; - case LAYOUT_8X: - rotors = 8; - dof = 4; - mix_matrix = mix_8x; - break; - case LAYOUT_6DOF_ROTORBITS: - rotors = 6; - dof = 6; - mix_matrix = mix_6dof_rotorbits; - break; - case LAYOUT_6DOF_5INCH_MONOCOQUE: - rotors = 6; - dof = 6; - mix_matrix = mix_6dof_5inch_monocoque; - break; - default: - fprintf(stderr,"ERROR in mix_init() unknown rotor layout\n"); - return -1; - } - - initialized = 1; - return 0; + switch (layout) + { + case LAYOUT_4X: + rotors = 4; + dof = 4; + mix_matrix = mix_4x; + break; + case LAYOUT_4PLUS: + rotors = 4; + dof = 4; + mix_matrix = mix_4plus; + break; + case LAYOUT_6X: + rotors = 6; + dof = 4; + mix_matrix = mix_6x; + break; + case LAYOUT_8X: + rotors = 8; + dof = 4; + mix_matrix = mix_8x; + break; + case LAYOUT_6DOF_ROTORBITS: + rotors = 6; + dof = 6; + mix_matrix = mix_6dof_rotorbits; + break; + case LAYOUT_6DOF_5INCH_MONOCOQUE: + rotors = 6; + dof = 6; + mix_matrix = mix_6dof_5inch_monocoque; + break; + default: + fprintf(stderr, "ERROR in mix_init() unknown rotor layout\n"); + return -1; + } + + initialized = 1; + return 0; } - int mix_all_controls(double u[6], double* mot) { - int i,j; - if(initialized!=1){ - fprintf(stderr,"ERROR in mix_all_controls, mixing matrix not set yet\n"); - return -1; - } - // sum control inputs - for(i=0;i1.0) mot[i]=1.0; - else if(mot[i]<0.0) mot[i]=0.0; - } - return 0; + int i, j; + if (initialized != 1) + { + fprintf(stderr, "ERROR in mix_all_controls, mixing matrix not set yet\n"); + return -1; + } + // sum control inputs + for (i = 0; i < rotors; i++) + { + mot[i] = 0.0; + for (j = 0; j < 6; j++) + { + mot[i] += mix_matrix[i][j] * u[j]; + } + } + // ensure saturation, should not need to do this if mix_check_saturation + // was used properly, but here for safety anyway. + for (i = 0; i < rotors; i++) + { + if (mot[i] > 1.0) + mot[i] = 1.0; + else if (mot[i] < 0.0) + mot[i] = 0.0; + } + return 0; } - int mix_check_saturation(int ch, double* mot, double* min, double* max) { - int i, min_ch; - double tmp; - double new_max = DBL_MAX; - double new_min = -DBL_MAX; - - if(initialized!=1){ - fprintf(stderr,"ERROR: in check_channel_saturation, mix matrix not set yet\n"); - return -1; - } - - switch(dof){ - case 4: - min_ch = 2; - break; - case 6: - min_ch = 0; - break; - default: - fprintf(stderr,"ERROR: in check_channel_saturation, dof should be 4 or 6, currently %d\n", dof); - return -1; - } - - if(ch=6){ - fprintf(stderr,"ERROR: in check_channel_saturation, ch out of bounds\n"); - return -1; - } - - // make sure motors are not already saturated - for(i=0;i1.0 || mot[i]<0.0){ - fprintf(stderr,"ERROR: motor channel already out of bounds\n"); - return -1; - } - } - - // find max positive input - for(i=0;i0.0) tmp = (1.0-mot[i])/mix_matrix[i][ch]; - // for negative entry in mix matrix - else tmp = -mot[i]/mix_matrix[i][ch]; - // set new upper limit if lower than current - if(tmp0.0) tmp = -mot[i]/mix_matrix[i][ch]; - // for negative entry in mix matrix - else tmp = (1.0-mot[i])/mix_matrix[i][ch]; - // set new upper limit if lower than current - if(tmp>new_min) new_min = tmp; - } - - *min = new_min; - *max = new_max; - return 0; + int i, min_ch; + double tmp; + double new_max = DBL_MAX; + double new_min = -DBL_MAX; + + if (initialized != 1) + { + fprintf(stderr, "ERROR: in check_channel_saturation, mix matrix not set yet\n"); + return -1; + } + + switch (dof) + { + case 4: + min_ch = 2; + break; + case 6: + min_ch = 0; + break; + default: + fprintf(stderr, + "ERROR: in check_channel_saturation, dof should be 4 or 6, currently %d\n", dof); + return -1; + } + + if (ch < min_ch || ch >= 6) + { + fprintf(stderr, "ERROR: in check_channel_saturation, ch out of bounds\n"); + return -1; + } + + // make sure motors are not already saturated + for (i = 0; i < rotors; i++) + { + if (mot[i] > 1.0 || mot[i] < 0.0) + { + fprintf(stderr, "ERROR: motor channel already out of bounds\n"); + return -1; + } + } + + // find max positive input + for (i = 0; i < rotors; i++) + { + // if mix channel is 0, impossible to saturate + if (mix_matrix[i][ch] == 0.0) continue; + // for positive entry in mix matrix + if (mix_matrix[i][ch] > 0.0) tmp = (1.0 - mot[i]) / mix_matrix[i][ch]; + // for negative entry in mix matrix + else + tmp = -mot[i] / mix_matrix[i][ch]; + // set new upper limit if lower than current + if (tmp < new_max) new_max = tmp; + } + + // find min (most negative) input + for (i = 0; i < rotors; i++) + { + // if mix channel is 0, impossible to saturate + if (mix_matrix[i][ch] == 0.0) continue; + // for positive entry in mix matrix + if (mix_matrix[i][ch] > 0.0) tmp = -mot[i] / mix_matrix[i][ch]; + // for negative entry in mix matrix + else + tmp = (1.0 - mot[i]) / mix_matrix[i][ch]; + // set new upper limit if lower than current + if (tmp > new_min) new_min = tmp; + } + + *min = new_min; + *max = new_max; + return 0; } - int mix_add_input(double u, int ch, double* mot) { - int i; - int min_ch; - - if(initialized!=1 || dof==0){ - fprintf(stderr,"ERROR: in mix_add_input, mix matrix not set yet\n"); - return -1; - } - switch(dof){ - case 4: - min_ch = 2; - break; - case 6: - min_ch = 0; - break; - default: - fprintf(stderr,"ERROR: in mix_add_input, dof should be 4 or 6, currently %d\n", dof); - return -1; - } - - if(ch=6){ - fprintf(stderr,"ERROR: in mix_add_input, ch out of bounds\n"); - return -1; - } - - // add inputs - for(i=0;i1.0) mot[i]=1.0; - else if(mot[i]<0.0) mot[i]=0.0; - } - return 0; + int i; + int min_ch; + + if (initialized != 1 || dof == 0) + { + fprintf(stderr, "ERROR: in mix_add_input, mix matrix not set yet\n"); + return -1; + } + switch (dof) + { + case 4: + min_ch = 2; + break; + case 6: + min_ch = 0; + break; + default: + fprintf(stderr, "ERROR: in mix_add_input, dof should be 4 or 6, currently %d\n", dof); + return -1; + } + + if (ch < min_ch || ch >= 6) + { + fprintf(stderr, "ERROR: in mix_add_input, ch out of bounds\n"); + return -1; + } + + // add inputs + for (i = 0; i < rotors; i++) + { + mot[i] += u * mix_matrix[i][ch]; + // ensure saturation, should not need to do this if mix_check_saturation + // was used properly, but here for safety anyway. + if (mot[i] > 1.0) + mot[i] = 1.0; + else if (mot[i] < 0.0) + mot[i] = 0.0; + } + return 0; } - - diff --git a/src/printf_manager.c b/src/printf_manager.c index a457211..284c8ea 100644 --- a/src/printf_manager.c +++ b/src/printf_manager.c @@ -2,30 +2,28 @@ * @file printf_manager.c */ +#include #include #include -#include +#include #include #include -#include -#include -#include +#include #include +#include +#include #include -#include +#include #include #include -#include - - static pthread_t printf_manager_thread; static int initialized = 0; const char* const colours[] = {KYEL, KCYN, KGRN, KMAG}; -const int num_colours = 4; // length of above array +const int num_colours = 4; // length of above array int current_colour = 0; /** @@ -35,209 +33,214 @@ int current_colour = 0; */ static const char* __next_colour() { - // if reached the end of the colour list, loop around - if(current_colour>=(num_colours-1)){ - current_colour=0; - return colours[num_colours-1]; - } - // else increment counter and return - current_colour++; - return colours[current_colour-1]; + // if reached the end of the colour list, loop around + if (current_colour >= (num_colours - 1)) + { + current_colour = 0; + return colours[num_colours - 1]; + } + // else increment counter and return + current_colour++; + return colours[current_colour - 1]; } static void __reset_colour() { - current_colour = 0; + current_colour = 0; } - static int __print_header() { - int i; - - printf("\n"); - __reset_colour(); - if(settings.printf_arm){ - printf(" arm |"); - } - if(settings.printf_altitude){ - printf("%s alt(m) |altdot|", __next_colour()); - } - if(settings.printf_rpy){ - printf("%s roll|pitch| yaw |", __next_colour()); - } - if(settings.printf_sticks){ - printf("%s kill | thr |roll |pitch| yaw |", __next_colour()); - } - if(settings.printf_setpoint){ - printf("%s sp_a | sp_r| sp_p| sp_y|", __next_colour()); - } - if(settings.printf_u){ - printf("%s U0X | U1Y | U2Z | U3r | U4p | U5y |", __next_colour()); - } - if(settings.printf_motors){ - printf("%s", __next_colour()); - for(i=0;i + * @file setpoint_manager.c + * + * + **/ #include -#include // for memset +#include +#include // for memset #include +#include +#include +#include +#include #include #include -#include -#include #include -#include -#include -#define XYZ_MAX_ERROR 0.5 ///< meters. - -setpoint_t setpoint; // extern variable in setpoint_manager.h +#define XYZ_MAX_ERROR 0.5 ///< meters. +setpoint_t setpoint; // extern variable in setpoint_manager.h void __update_yaw(void) { - // if throttle stick is down all the way, probably landed, so - // keep the yaw setpoint at current yaw so it takes off straight - if(user_input.thr_stick < -0.95){ - setpoint.yaw = state_estimate.yaw; - setpoint.yaw_dot = 0.0; - return; - } - // otherwise, scale yaw_rate by max yaw rate in rad/s - // and move yaw setpoint - setpoint.yaw_dot = user_input.yaw_stick * MAX_YAW_RATE; - setpoint.yaw += setpoint.yaw_dot*DT; - return; + // if throttle stick is down all the way, probably landed, so + // keep the yaw setpoint at current yaw so it takes off straight + if (user_input.thr_stick < -0.95) + { + setpoint.yaw = state_estimate.yaw; + setpoint.yaw_dot = 0.0; + return; + } + // otherwise, scale yaw_rate by max yaw rate in rad/s + // and move yaw setpoint + setpoint.yaw_dot = user_input.yaw_stick * MAX_YAW_RATE; + setpoint.yaw += setpoint.yaw_dot * DT; + return; } void __update_Z(void) { - // make sure setpoint doesn't go too far below current altitude since we - // can't sink into the ground - if(setpoint.Z > (state_estimate.Z + XYZ_MAX_ERROR)){ - setpoint.Z = state_estimate.Z + XYZ_MAX_ERROR; - setpoint.Z_dot = 0.0; - return; - } - setpoint.Z_dot = -user_input.thr_stick * settings.max_Z_velocity; - setpoint.Z += setpoint.Z_dot*DT; - return; + // make sure setpoint doesn't go too far below current altitude since we + // can't sink into the ground + if (setpoint.Z > (state_estimate.Z + XYZ_MAX_ERROR)) + { + setpoint.Z = state_estimate.Z + XYZ_MAX_ERROR; + setpoint.Z_dot = 0.0; + return; + } + setpoint.Z_dot = -user_input.thr_stick * settings.max_Z_velocity; + setpoint.Z += setpoint.Z_dot * DT; + return; } void __update_XY_pos(void) { - // make sure setpoint doesn't go too far from state in case touching something - if(setpoint.X > (state_estimate.X + XYZ_MAX_ERROR)){ - setpoint.X = state_estimate.X + XYZ_MAX_ERROR; - setpoint.X_dot = 0.0; - } - else if(setpoint.X < (state_estimate.X - XYZ_MAX_ERROR)){ - setpoint.X = state_estimate.X - XYZ_MAX_ERROR; - setpoint.X_dot = 0.0; - return; - } - else{ - setpoint.X += setpoint.X_dot*DT; - } - - if(setpoint.Y > (state_estimate.Y + XYZ_MAX_ERROR)){ - setpoint.Y = state_estimate.Y + XYZ_MAX_ERROR; - setpoint.Y_dot = 0.0; - return; - } - else if(setpoint.Y < (state_estimate.Y - XYZ_MAX_ERROR)){ - setpoint.Y = state_estimate.Y - XYZ_MAX_ERROR; - setpoint.Y_dot = 0.0; - return; - } - else{ - setpoint.Y += setpoint.Y_dot*DT; - } - - return; + // make sure setpoint doesn't go too far from state in case touching something + if (setpoint.X > (state_estimate.X + XYZ_MAX_ERROR)) + { + setpoint.X = state_estimate.X + XYZ_MAX_ERROR; + setpoint.X_dot = 0.0; + } + else if (setpoint.X < (state_estimate.X - XYZ_MAX_ERROR)) + { + setpoint.X = state_estimate.X - XYZ_MAX_ERROR; + setpoint.X_dot = 0.0; + return; + } + else + { + setpoint.X += setpoint.X_dot * DT; + } + + if (setpoint.Y > (state_estimate.Y + XYZ_MAX_ERROR)) + { + setpoint.Y = state_estimate.Y + XYZ_MAX_ERROR; + setpoint.Y_dot = 0.0; + return; + } + else if (setpoint.Y < (state_estimate.Y - XYZ_MAX_ERROR)) + { + setpoint.Y = state_estimate.Y - XYZ_MAX_ERROR; + setpoint.Y_dot = 0.0; + return; + } + else + { + setpoint.Y += setpoint.Y_dot * DT; + } + + return; } - int setpoint_manager_init(void) { - if(setpoint.initialized){ - fprintf(stderr, "ERROR in setpoint_manager_init, already initialized\n"); - return -1; - } - memset(&setpoint,0,sizeof(setpoint_t)); - setpoint.initialized = 1; - return 0; + if (setpoint.initialized) + { + fprintf(stderr, "ERROR in setpoint_manager_init, already initialized\n"); + return -1; + } + memset(&setpoint, 0, sizeof(setpoint_t)); + setpoint.initialized = 1; + return 0; } - - int setpoint_manager_update(void) { - if(setpoint.initialized==0){ - fprintf(stderr, "ERROR in setpoint_manager_update, not initialized yet\n"); - return -1; - } - - if(user_input.initialized==0){ - fprintf(stderr, "ERROR in setpoint_manager_update, input_manager not initialized yet\n"); - return -1; - } - - // if PAUSED or UNINITIALIZED, do nothing - if(rc_get_state()!=RUNNING) return 0; - - // shutdown feedback on kill switch - if(user_input.requested_arm_mode == DISARMED){ - if(fstate.arm_state==ARMED) feedback_disarm(); - return 0; - } - - // finally, switch between flight modes and adjust setpoint properly - switch(user_input.flight_mode){ - - - case TEST_BENCH_4DOF: - // configure which controllers are enabled - setpoint.en_6dof = 0; - setpoint.en_rpy_ctrl = 0; - setpoint.en_Z_ctrl = 0; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.roll_throttle = user_input.roll_stick; - setpoint.pitch_throttle = user_input.pitch_stick; - setpoint.yaw_throttle = user_input.yaw_stick; - setpoint.Z_throttle = -user_input.thr_stick; - // TODO add these two throttle modes as options to settings, I use a radio - // with self-centering throttle so having 0 in the middle is safest - // setpoint.Z_throttle = -(user_input.thr_stick+1.0)/2.0; - break; - - case TEST_BENCH_6DOF: - setpoint.en_6dof = 1; - setpoint.en_rpy_ctrl = 0; - setpoint.en_Z_ctrl = 0; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.X_throttle = -user_input.pitch_stick; - setpoint.Y_throttle = user_input.roll_stick; - setpoint.roll_throttle = 0.0; - setpoint.pitch_throttle = 0.0; - setpoint.yaw_throttle = user_input.yaw_stick; - setpoint.Z_throttle = -user_input.thr_stick; - break; - - case DIRECT_THROTTLE_4DOF: - setpoint.en_6dof = 0; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 0; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.roll = user_input.roll_stick; - setpoint.pitch = user_input.pitch_stick; - setpoint.Z_throttle = -user_input.thr_stick; - __update_yaw(); - break; - - case DIRECT_THROTTLE_6DOF: - setpoint.en_6dof = 1; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 0; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.X_throttle = -user_input.pitch_stick; - setpoint.Y_throttle = user_input.roll_stick; - setpoint.Z_throttle = -user_input.thr_stick; - __update_yaw(); - break; - - case ALT_HOLD_4DOF: - setpoint.en_6dof = 0; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.roll = user_input.roll_stick; - setpoint.pitch = user_input.pitch_stick; - __update_Z(); - __update_yaw(); - break; - - case ALT_HOLD_6DOF: - setpoint.en_6dof = 1; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.roll = 0.0; - setpoint.pitch = 0.0; - setpoint.X_throttle = -user_input.pitch_stick; - setpoint.Y_throttle = user_input.roll_stick; - __update_Z(); - __update_yaw(); - break; - - case VELOCITY_CONTROL_4DOF: - setpoint.en_6dof = 0; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 1; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; - setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; - __update_Z(); - __update_yaw(); - break; - - case VELOCITY_CONTROL_6DOF: - setpoint.en_6dof = 1; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 1; - setpoint.en_XY_pos_ctrl = 0; - - setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; - setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; - __update_Z(); - __update_yaw(); - break; - - case POSITION_CONTROL_4DOF: - setpoint.en_6dof = 0; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 1; - - setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; - setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; - __update_XY_pos(); - __update_Z(); - __update_yaw(); - break; - - case POSITION_CONTROL_6DOF: - setpoint.en_6dof = 1; - setpoint.en_rpy_ctrl = 1; - setpoint.en_Z_ctrl = 1; - setpoint.en_XY_vel_ctrl = 0; - setpoint.en_XY_pos_ctrl = 1; - - setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; - setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; - __update_XY_pos(); - __update_Z(); - __update_yaw(); - break; - - default: // should never get here - fprintf(stderr,"ERROR in setpoint_manager thread, unknown flight mode\n"); - break; - - } // end switch(user_input.flight_mode) - - // arm feedback when requested - if(user_input.requested_arm_mode == ARMED){ - if(fstate.arm_state==DISARMED) feedback_arm(); - } - - return 0; + if (setpoint.initialized == 0) + { + fprintf(stderr, "ERROR in setpoint_manager_update, not initialized yet\n"); + return -1; + } + + if (user_input.initialized == 0) + { + fprintf(stderr, "ERROR in setpoint_manager_update, input_manager not initialized yet\n"); + return -1; + } + + // if PAUSED or UNINITIALIZED, do nothing + if (rc_get_state() != RUNNING) return 0; + + // shutdown feedback on kill switch + if (user_input.requested_arm_mode == DISARMED) + { + if (fstate.arm_state == ARMED) feedback_disarm(); + return 0; + } + + // finally, switch between flight modes and adjust setpoint properly + switch (user_input.flight_mode) + { + case TEST_BENCH_4DOF: + // configure which controllers are enabled + setpoint.en_6dof = 0; + setpoint.en_rpy_ctrl = 0; + setpoint.en_Z_ctrl = 0; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.roll_throttle = user_input.roll_stick; + setpoint.pitch_throttle = user_input.pitch_stick; + setpoint.yaw_throttle = user_input.yaw_stick; + setpoint.Z_throttle = -user_input.thr_stick; + // TODO add these two throttle modes as options to settings, I use a radio + // with self-centering throttle so having 0 in the middle is safest + // setpoint.Z_throttle = -(user_input.thr_stick+1.0)/2.0; + break; + + case TEST_BENCH_6DOF: + setpoint.en_6dof = 1; + setpoint.en_rpy_ctrl = 0; + setpoint.en_Z_ctrl = 0; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.X_throttle = -user_input.pitch_stick; + setpoint.Y_throttle = user_input.roll_stick; + setpoint.roll_throttle = 0.0; + setpoint.pitch_throttle = 0.0; + setpoint.yaw_throttle = user_input.yaw_stick; + setpoint.Z_throttle = -user_input.thr_stick; + break; + + case DIRECT_THROTTLE_4DOF: + setpoint.en_6dof = 0; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 0; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.roll = user_input.roll_stick; + setpoint.pitch = user_input.pitch_stick; + setpoint.Z_throttle = -user_input.thr_stick; + __update_yaw(); + break; + + case DIRECT_THROTTLE_6DOF: + setpoint.en_6dof = 1; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 0; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.X_throttle = -user_input.pitch_stick; + setpoint.Y_throttle = user_input.roll_stick; + setpoint.Z_throttle = -user_input.thr_stick; + __update_yaw(); + break; + + case ALT_HOLD_4DOF: + setpoint.en_6dof = 0; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.roll = user_input.roll_stick; + setpoint.pitch = user_input.pitch_stick; + __update_Z(); + __update_yaw(); + break; + + case ALT_HOLD_6DOF: + setpoint.en_6dof = 1; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.roll = 0.0; + setpoint.pitch = 0.0; + setpoint.X_throttle = -user_input.pitch_stick; + setpoint.Y_throttle = user_input.roll_stick; + __update_Z(); + __update_yaw(); + break; + + case VELOCITY_CONTROL_4DOF: + setpoint.en_6dof = 0; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 1; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; + setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; + __update_Z(); + __update_yaw(); + break; + + case VELOCITY_CONTROL_6DOF: + setpoint.en_6dof = 1; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 1; + setpoint.en_XY_pos_ctrl = 0; + + setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; + setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; + __update_Z(); + __update_yaw(); + break; + + case POSITION_CONTROL_4DOF: + setpoint.en_6dof = 0; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 1; + + setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; + setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; + __update_XY_pos(); + __update_Z(); + __update_yaw(); + break; + + case POSITION_CONTROL_6DOF: + setpoint.en_6dof = 1; + setpoint.en_rpy_ctrl = 1; + setpoint.en_Z_ctrl = 1; + setpoint.en_XY_vel_ctrl = 0; + setpoint.en_XY_pos_ctrl = 1; + + setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; + setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; + __update_XY_pos(); + __update_Z(); + __update_yaw(); + break; + + default: // should never get here + fprintf(stderr, "ERROR in setpoint_manager thread, unknown flight mode\n"); + break; + + } // end switch(user_input.flight_mode) + + // arm feedback when requested + if (user_input.requested_arm_mode == ARMED) + { + if (fstate.arm_state == DISARMED) feedback_arm(); + } + + return 0; } - int setpoint_manager_cleanup(void) { - setpoint.initialized=0; - return 0; + setpoint.initialized = 0; + return 0; } diff --git a/src/settings.c b/src/settings.c index 7efe740..0a3a7bc 100644 --- a/src/settings.c +++ b/src/settings.c @@ -1,21 +1,20 @@ /** -* @file settings.c -* -* contains all the functions for io to the settings file, including default -* values that can be written to disk if no file is present. + * @file settings.c + * + * contains all the functions for io to the settings file, including default + * values that can be written to disk if no file is present. **/ +#include // for F_OK #include -#include // FOR str_cmp() -#include // for F_OK -#include // for access() +#include // FOR str_cmp() +#include // for access() #include #include -#include #include - +#include // json object respresentation of the whole settings file static json_object* jobj; @@ -31,105 +30,116 @@ static int was_load_successful = 0; //////////////////////////////////////////////////////////////////////////////// // macro for reading a boolean -#define PARSE_BOOL(name) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){\ - fprintf(stderr,"ERROR parsing settings file, can't find " #name "\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_boolean)==0){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be a boolean\n");\ - return -1;\ -}\ -settings.name = json_object_get_boolean(tmp);\ - +#define PARSE_BOOL(name) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_boolean) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be a boolean\n"); \ + return -1; \ + } \ + settings.name = json_object_get_boolean(tmp); // macro for reading an integer -#define PARSE_INT(name) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){ \ - fprintf(stderr,"ERROR parsing settings file, can't find " #name "\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_int)==0){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be an int\n");\ - return -1;\ -}\ -settings.name = json_object_get_int(tmp);\ - +#define PARSE_INT(name) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_int) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ + return -1; \ + } \ + settings.name = json_object_get_int(tmp); // macro for reading a bound integer -#define PARSE_INT_MIN_MAX(name,min,max) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){ \ - fprintf(stderr,"ERROR parsing settings file, can't find " #name "\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_int)==0){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be an int\n");\ - return -1;\ -}\ -settings.name = json_object_get_int(tmp);\ -if(settings.namemax){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be between min and max\n");\ - return -1;\ -}\ - +#define PARSE_INT_MIN_MAX(name, min, max) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_int) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ + return -1; \ + } \ + settings.name = json_object_get_int(tmp); \ + if (settings.name < min || settings.name > max) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be between min and max\n"); \ + return -1; \ + } // macro for reading a polarity which should be +-1 -#define PARSE_POLARITY(name) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){ \ - fprintf(stderr,"ERROR parsing settings file, can't find " #name "\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_int)==0){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be an int\n");\ - return -1;\ -}\ -settings.name = json_object_get_int(tmp);\ -if(settings.name!=-1 && settings.name!=1){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be -1 or 1\n");\ - return -1;\ -}\ - +#define PARSE_POLARITY(name) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_int) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ + return -1; \ + } \ + settings.name = json_object_get_int(tmp); \ + if (settings.name != -1 && settings.name != 1) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be -1 or 1\n"); \ + return -1; \ + } // macro for reading a floating point number -#define PARSE_DOUBLE_MIN_MAX(name,min,max)\ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){\ - fprintf(stderr,"ERROR can't find " #name " in settings file\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_double)==0){\ - fprintf(stderr,"ERROR " #name " should be a double\n");\ - return -1;\ -}\ -settings.name = json_object_get_double(tmp);\ -if(settings.namemax){\ - fprintf(stderr,"ERROR " #name " should be between min and max\n");\ - return -1;\ -}\ - +#define PARSE_DOUBLE_MIN_MAX(name, min, max) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR can't find " #name " in settings file\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_double) == 0) \ + { \ + fprintf(stderr, "ERROR " #name " should be a double\n"); \ + return -1; \ + } \ + settings.name = json_object_get_double(tmp); \ + if (settings.name < min || settings.name > max) \ + { \ + fprintf(stderr, "ERROR " #name " should be between min and max\n"); \ + return -1; \ + } // macro for reading a string -#define PARSE_STRING(name) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){ \ - fprintf(stderr,"ERROR parsing settings file, can't find " #name "\n");\ - return -1;\ -}\ -if(json_object_is_type(tmp, json_type_string)==0){\ - fprintf(stderr,"ERROR parsing settings file, " #name " should be a string\n");\ - return -1;\ -}\ -strcpy(settings.name, json_object_get_string(tmp)); +#define PARSE_STRING(name) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ + return -1; \ + } \ + if (json_object_is_type(tmp, json_type_string) == 0) \ + { \ + fprintf(stderr, "ERROR parsing settings file, " #name " should be a string\n"); \ + return -1; \ + } \ + strcpy(settings.name, json_object_get_string(tmp)); // macro for reading feedback controller -#define PARSE_CONTROLLER(name) \ -if(json_object_object_get_ex(jobj, #name, &tmp)==0){\ - fprintf(stderr,"ERROR: can't find " #name " in settings file\n");\ - return -1;\ -}\ -if(__parse_controller(tmp, &settings.name)){\ - fprintf(stderr,"ERROR: could not parse " #name "\n");\ - return -1;\ -}\ - +#define PARSE_CONTROLLER(name) \ + if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ + { \ + fprintf(stderr, "ERROR: can't find " #name " in settings file\n"); \ + return -1; \ + } \ + if (__parse_controller(tmp, &settings.name)) \ + { \ + fprintf(stderr, "ERROR: could not parse " #name "\n"); \ + return -1; \ + } //////////////////////////////////////////////////////////////////////////////// /// functions for parsing enums @@ -142,78 +152,91 @@ if(__parse_controller(tmp, &settings.name)){\ */ static int __parse_layout(void) { - struct json_object *tmp = NULL; - char* tmp_str = NULL; - if(json_object_object_get_ex(jobj, "layout", &tmp)==0){ - fprintf(stderr,"ERROR: can't find layout in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_string)==0){ - fprintf(stderr,"ERROR: layout should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(tmp); - if(strcmp(tmp_str, "LAYOUT_6DOF_ROTORBITS")==0){ - settings.num_rotors = 6; - settings.layout = LAYOUT_6DOF_ROTORBITS; - } - else if(strcmp(tmp_str, "LAYOUT_4X")==0){ - settings.num_rotors = 4; - settings.layout = LAYOUT_4X; - } - else if(strcmp(tmp_str, "LAYOUT_4PLUS")==0){ - settings.num_rotors = 4; - settings.layout = LAYOUT_4PLUS; - } - else if(strcmp(tmp_str, "LAYOUT_6X")==0){ - settings.num_rotors = 6; - settings.layout = LAYOUT_6X; - } - else if(strcmp(tmp_str, "LAYOUT_8X")==0){ - settings.num_rotors = 8; - settings.layout = LAYOUT_8X; - } - else{ - fprintf(stderr,"ERROR: invalid layout string\n"); - return -1; - } - return 0; + struct json_object* tmp = NULL; + char* tmp_str = NULL; + if (json_object_object_get_ex(jobj, "layout", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find layout in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_string) == 0) + { + fprintf(stderr, "ERROR: layout should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(tmp); + if (strcmp(tmp_str, "LAYOUT_6DOF_ROTORBITS") == 0) + { + settings.num_rotors = 6; + settings.layout = LAYOUT_6DOF_ROTORBITS; + } + else if (strcmp(tmp_str, "LAYOUT_4X") == 0) + { + settings.num_rotors = 4; + settings.layout = LAYOUT_4X; + } + else if (strcmp(tmp_str, "LAYOUT_4PLUS") == 0) + { + settings.num_rotors = 4; + settings.layout = LAYOUT_4PLUS; + } + else if (strcmp(tmp_str, "LAYOUT_6X") == 0) + { + settings.num_rotors = 6; + settings.layout = LAYOUT_6X; + } + else if (strcmp(tmp_str, "LAYOUT_8X") == 0) + { + settings.num_rotors = 8; + settings.layout = LAYOUT_8X; + } + else + { + fprintf(stderr, "ERROR: invalid layout string\n"); + return -1; + } + return 0; } - static int __parse_thrust_map(void) { - struct json_object *tmp = NULL; - char* tmp_str = NULL; - if(json_object_object_get_ex(jobj, "thrust_map", &tmp)==0){ - fprintf(stderr,"ERROR: can't find thrust_map in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_string)==0){ - fprintf(stderr,"ERROR: thrust map should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(tmp); - if(strcmp(tmp_str, "LINEAR_MAP")==0){ - settings.thrust_map = LINEAR_MAP; - } - else if(strcmp(tmp_str, "MN1806_1400KV_4S")==0){ - settings.thrust_map = MN1806_1400KV_4S; - } - else if(strcmp(tmp_str, "F20_2300KV_2S")==0){ - settings.thrust_map = F20_2300KV_2S; - } - else if(strcmp(tmp_str, "RX2206_4S")==0){ - settings.thrust_map = RX2206_4S; - } - else{ - fprintf(stderr,"ERROR: invalid thrust_map string\n"); - return -1; - } - return 0; + struct json_object* tmp = NULL; + char* tmp_str = NULL; + if (json_object_object_get_ex(jobj, "thrust_map", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find thrust_map in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_string) == 0) + { + fprintf(stderr, "ERROR: thrust map should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(tmp); + if (strcmp(tmp_str, "LINEAR_MAP") == 0) + { + settings.thrust_map = LINEAR_MAP; + } + else if (strcmp(tmp_str, "MN1806_1400KV_4S") == 0) + { + settings.thrust_map = MN1806_1400KV_4S; + } + else if (strcmp(tmp_str, "F20_2300KV_2S") == 0) + { + settings.thrust_map = F20_2300KV_2S; + } + else if (strcmp(tmp_str, "RX2206_4S") == 0) + { + settings.thrust_map = RX2206_4S; + } + else + { + fprintf(stderr, "ERROR: invalid thrust_map string\n"); + return -1; + } + return 0; } - /** * @brief parses a json_object and fills in the flight mode. * @@ -224,79 +247,93 @@ static int __parse_thrust_map(void) */ static int __parse_flight_mode(json_object* jobj_str, flight_mode_t* mode) { - char* tmp_str = NULL; - struct json_object *tmp = NULL; - if(json_object_is_type(jobj_str, json_type_string)==0){ - fprintf(stderr,"ERROR: flight_mode should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(jobj_str); - if(strcmp(tmp_str, "TEST_BENCH_4DOF")==0){ - *mode = TEST_BENCH_4DOF; - } - else if(strcmp(tmp_str, "TEST_BENCH_6DOF")==0){ - *mode = TEST_BENCH_6DOF; - } - else if(strcmp(tmp_str, "DIRECT_THROTTLE_4DOF")==0){ - *mode = DIRECT_THROTTLE_4DOF; - } - else if(strcmp(tmp_str, "DIRECT_THROTTLE_6DOF")==0){ - *mode = DIRECT_THROTTLE_6DOF; - } - else if(strcmp(tmp_str, "ALT_HOLD_4DOF")==0){ - *mode = ALT_HOLD_4DOF; - } - else if(strcmp(tmp_str, "ALT_HOLD_6DOF")==0){ - *mode = ALT_HOLD_6DOF; - } - else if(strcmp(tmp_str, "VELOCITY_CONTROL_4DOF")==0){ - *mode = VELOCITY_CONTROL_4DOF; - } - else if(strcmp(tmp_str, "VELOCITY_CONTROL_6DOF")==0){ - *mode = VELOCITY_CONTROL_6DOF; - } - else if(strcmp(tmp_str, "POSITION_CONTROL_4DOF")==0){ - *mode = POSITION_CONTROL_4DOF; - } - else if(strcmp(tmp_str, "POSITION_CONTROL_6DOF")==0){ - *mode = POSITION_CONTROL_6DOF; - } - else{ - fprintf(stderr,"ERROR: invalid flight mode\n"); - return -1; - } - return 0; + char* tmp_str = NULL; + struct json_object* tmp = NULL; + if (json_object_is_type(jobj_str, json_type_string) == 0) + { + fprintf(stderr, "ERROR: flight_mode should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(jobj_str); + if (strcmp(tmp_str, "TEST_BENCH_4DOF") == 0) + { + *mode = TEST_BENCH_4DOF; + } + else if (strcmp(tmp_str, "TEST_BENCH_6DOF") == 0) + { + *mode = TEST_BENCH_6DOF; + } + else if (strcmp(tmp_str, "DIRECT_THROTTLE_4DOF") == 0) + { + *mode = DIRECT_THROTTLE_4DOF; + } + else if (strcmp(tmp_str, "DIRECT_THROTTLE_6DOF") == 0) + { + *mode = DIRECT_THROTTLE_6DOF; + } + else if (strcmp(tmp_str, "ALT_HOLD_4DOF") == 0) + { + *mode = ALT_HOLD_4DOF; + } + else if (strcmp(tmp_str, "ALT_HOLD_6DOF") == 0) + { + *mode = ALT_HOLD_6DOF; + } + else if (strcmp(tmp_str, "VELOCITY_CONTROL_4DOF") == 0) + { + *mode = VELOCITY_CONTROL_4DOF; + } + else if (strcmp(tmp_str, "VELOCITY_CONTROL_6DOF") == 0) + { + *mode = VELOCITY_CONTROL_6DOF; + } + else if (strcmp(tmp_str, "POSITION_CONTROL_4DOF") == 0) + { + *mode = POSITION_CONTROL_4DOF; + } + else if (strcmp(tmp_str, "POSITION_CONTROL_6DOF") == 0) + { + *mode = POSITION_CONTROL_6DOF; + } + else + { + fprintf(stderr, "ERROR: invalid flight mode\n"); + return -1; + } + return 0; } - - static int __parse_kill_mode(void) { - struct json_object *tmp = NULL; - char* tmp_str = NULL; - if(json_object_object_get_ex(jobj, "dsm_kill_mode", &tmp)==0){ - fprintf(stderr,"ERROR: can't find dsm_kill_mode in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_string)==0){ - fprintf(stderr,"ERROR: dsm_kill_mode should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(tmp); - if(strcmp(tmp_str, "DSM_KILL_DEDICATED_SWITCH")==0){ - settings.dsm_kill_mode = DSM_KILL_DEDICATED_SWITCH; - } - else if(strcmp(tmp_str, "DSM_KILL_NEGATIVE_THROTTLE")==0){ - settings.dsm_kill_mode = DSM_KILL_NEGATIVE_THROTTLE; - } - else{ - fprintf(stderr,"ERROR: invalid dsm_kill_mode string\n"); - return -1; - } - return 0; + struct json_object* tmp = NULL; + char* tmp_str = NULL; + if (json_object_object_get_ex(jobj, "dsm_kill_mode", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find dsm_kill_mode in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_string) == 0) + { + fprintf(stderr, "ERROR: dsm_kill_mode should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(tmp); + if (strcmp(tmp_str, "DSM_KILL_DEDICATED_SWITCH") == 0) + { + settings.dsm_kill_mode = DSM_KILL_DEDICATED_SWITCH; + } + else if (strcmp(tmp_str, "DSM_KILL_NEGATIVE_THROTTLE") == 0) + { + settings.dsm_kill_mode = DSM_KILL_NEGATIVE_THROTTLE; + } + else + { + fprintf(stderr, "ERROR: invalid dsm_kill_mode string\n"); + return -1; + } + return 0; } - /** * @ brief parses a json_object and sets up a new controller * @@ -307,353 +344,380 @@ static int __parse_kill_mode(void) */ static int __parse_controller(json_object* jobj_ctl, rc_filter_t* filter) { - struct json_object *array = NULL; // to hold num & den arrays - struct json_object *tmp = NULL; // temp object - char* tmp_str = NULL; - double tmp_flt, tmp_kp, tmp_ki, tmp_kd; - int i, num_len, den_len; - rc_vector_t num_vec = RC_VECTOR_INITIALIZER; - rc_vector_t den_vec = RC_VECTOR_INITIALIZER; - - // destroy old memory in case the order changes - rc_filter_free(filter); - - // pull out gain - if(json_object_object_get_ex(jobj_ctl, "gain", &tmp)==0){ - fprintf(stderr,"ERROR: can't find controller gain in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_double)==0){ - fprintf(stderr,"ERROR: controller gain should be a double\n"); - return -1; - } - tmp_flt = json_object_get_double(tmp); - - // check if PID gains or transfer function coefficients - if(json_object_object_get_ex(jobj_ctl, "TF_or_PID", &tmp)==0){ - fprintf(stderr,"ERROR: can't find TF_or_PID in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_string)==0){ - fprintf(stderr,"ERROR: TF_or_PID should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(tmp); - - if(strcmp(tmp_str, "TF")==0){ - - // pull out numerator - if(json_object_object_get_ex(jobj_ctl, "numerator", &array)==0){ - fprintf(stderr,"ERROR: can't find controller numerator in settings file\n"); - return -1; - } - if(json_object_is_type(array, json_type_array)==0){ - fprintf(stderr,"ERROR: controller numerator should be an array\n"); - return -1; - } - num_len = json_object_array_length(array); - if(num_len<1){ - fprintf(stderr,"ERROR, numerator must have at least 1 entry\n"); - return -1; - } - rc_vector_alloc(&num_vec,num_len); - for(i=0;iden_len){ - fprintf(stderr,"ERROR: improper transfer function\n"); - rc_vector_free(&num_vec); - rc_vector_free(&den_vec); - return -1; - } - - // check CT continuous time or DT discrete time - if(json_object_object_get_ex(jobj_ctl, "CT_or_DT", &tmp)==0){ - fprintf(stderr,"ERROR: can't find CT_or_DT in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_string)==0){ - fprintf(stderr,"ERROR: CT_or_DT should be a string\n"); - return -1; - } - tmp_str = (char*)json_object_get_string(tmp); - - - // if CT, use tustin's approx to get to DT - if(strcmp(tmp_str, "CT")==0){ - // get the crossover frequency - if(json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp)==0){ - fprintf(stderr,"ERROR: can't find crossover frequency in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_double)==0){ - fprintf(stderr,"ERROR: crossover frequency should be a double\n"); - return -1; - } - tmp_flt = json_object_get_double(tmp); - if(rc_filter_c2d_tustin(filter, DT, num_vec, den_vec, tmp_flt)){ - fprintf(stderr,"ERROR: failed to c2dtustin while parsing json\n"); - return -1; - } - } - - // if DT, much easier, just construct filter - else if(strcmp(tmp_str, "DT")==0){ - if(rc_filter_alloc(filter,num_vec, den_vec, DT)){ - fprintf(stderr,"ERROR: failed to alloc filter in __parse_controller()"); - return -1; - } - } - - // wrong value for CT_or_DT - else{ - fprintf(stderr,"ERROR: CT_or_DT must be 'CT' or 'DT'\n"); - printf("instead got :%s\n", tmp_str); - return -1; - } - } - - else if(strcmp(tmp_str, "PID")==0){ - // pull out gains - if(json_object_object_get_ex(jobj_ctl, "kp", &tmp)==0){ - fprintf(stderr,"ERROR: can't find kp in settings file\n"); - return -1; - } - tmp_kp = json_object_get_double(tmp); - if(json_object_object_get_ex(jobj_ctl, "ki", &tmp)==0){ - fprintf(stderr,"ERROR: can't find ki in settings file\n"); - return -1; - } - tmp_ki = json_object_get_double(tmp); - if(json_object_object_get_ex(jobj_ctl, "kd", &tmp)==0){ - fprintf(stderr,"ERROR: can't find kd in settings file\n"); - return -1; - } - tmp_kd = json_object_get_double(tmp); - // get the crossover frequency - if(json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp)==0){ - fprintf(stderr,"ERROR: can't find crossover frequency in settings file\n"); - return -1; - } - if(json_object_is_type(tmp, json_type_double)==0){ - fprintf(stderr,"ERROR: crossover frequency should be a double\n"); - return -1; - } - tmp_flt = json_object_get_double(tmp); - if(rc_filter_pid(filter,tmp_kp,tmp_ki,tmp_kd,1.0/tmp_flt, DT)){ - fprintf(stderr,"ERROR: failed to alloc pid filter in __parse_controller()"); - return -1; - } - } - - #ifdef DEBUG - rc_filter_print(*filter); - #endif - - rc_vector_free(&num_vec); - rc_vector_free(&den_vec); - - return 0; + struct json_object* array = NULL; // to hold num & den arrays + struct json_object* tmp = NULL; // temp object + char* tmp_str = NULL; + double tmp_flt, tmp_kp, tmp_ki, tmp_kd; + int i, num_len, den_len; + rc_vector_t num_vec = RC_VECTOR_INITIALIZER; + rc_vector_t den_vec = RC_VECTOR_INITIALIZER; + + // destroy old memory in case the order changes + rc_filter_free(filter); + + // pull out gain + if (json_object_object_get_ex(jobj_ctl, "gain", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find controller gain in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_double) == 0) + { + fprintf(stderr, "ERROR: controller gain should be a double\n"); + return -1; + } + tmp_flt = json_object_get_double(tmp); + + // check if PID gains or transfer function coefficients + if (json_object_object_get_ex(jobj_ctl, "TF_or_PID", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find TF_or_PID in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_string) == 0) + { + fprintf(stderr, "ERROR: TF_or_PID should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(tmp); + + if (strcmp(tmp_str, "TF") == 0) + { + // pull out numerator + if (json_object_object_get_ex(jobj_ctl, "numerator", &array) == 0) + { + fprintf(stderr, "ERROR: can't find controller numerator in settings file\n"); + return -1; + } + if (json_object_is_type(array, json_type_array) == 0) + { + fprintf(stderr, "ERROR: controller numerator should be an array\n"); + return -1; + } + num_len = json_object_array_length(array); + if (num_len < 1) + { + fprintf(stderr, "ERROR, numerator must have at least 1 entry\n"); + return -1; + } + rc_vector_alloc(&num_vec, num_len); + for (i = 0; i < num_len; i++) + { + tmp = json_object_array_get_idx(array, i); + if (json_object_is_type(tmp, json_type_double) == 0) + { + fprintf(stderr, "ERROR: numerator array entries should be a doubles\n"); + return -1; + } + tmp_flt = json_object_get_double(tmp); + num_vec.d[i] = tmp_flt; + } + + // pull out denominator + if (json_object_object_get_ex(jobj_ctl, "denominator", &array) == 0) + { + fprintf(stderr, "ERROR: can't find controller denominator in settings file\n"); + return -1; + } + if (json_object_is_type(array, json_type_array) == 0) + { + fprintf(stderr, "ERROR: controller denominator should be an array\n"); + return -1; + } + den_len = json_object_array_length(array); + if (den_len < 1) + { + fprintf(stderr, "ERROR, denominator must have at least 1 entry\n"); + return -1; + } + rc_vector_alloc(&den_vec, den_len); + for (i = 0; i < den_len; i++) + { + tmp = json_object_array_get_idx(array, i); + if (json_object_is_type(tmp, json_type_double) == 0) + { + fprintf(stderr, "ERROR: denominator array entries should be a doubles\n"); + return -1; + } + tmp_flt = json_object_get_double(tmp); + den_vec.d[i] = tmp_flt; + } + + // check for improper TF + if (num_len > den_len) + { + fprintf(stderr, "ERROR: improper transfer function\n"); + rc_vector_free(&num_vec); + rc_vector_free(&den_vec); + return -1; + } + + // check CT continuous time or DT discrete time + if (json_object_object_get_ex(jobj_ctl, "CT_or_DT", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find CT_or_DT in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_string) == 0) + { + fprintf(stderr, "ERROR: CT_or_DT should be a string\n"); + return -1; + } + tmp_str = (char*)json_object_get_string(tmp); + + // if CT, use tustin's approx to get to DT + if (strcmp(tmp_str, "CT") == 0) + { + // get the crossover frequency + if (json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find crossover frequency in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_double) == 0) + { + fprintf(stderr, "ERROR: crossover frequency should be a double\n"); + return -1; + } + tmp_flt = json_object_get_double(tmp); + if (rc_filter_c2d_tustin(filter, DT, num_vec, den_vec, tmp_flt)) + { + fprintf(stderr, "ERROR: failed to c2dtustin while parsing json\n"); + return -1; + } + } + + // if DT, much easier, just construct filter + else if (strcmp(tmp_str, "DT") == 0) + { + if (rc_filter_alloc(filter, num_vec, den_vec, DT)) + { + fprintf(stderr, "ERROR: failed to alloc filter in __parse_controller()"); + return -1; + } + } + + // wrong value for CT_or_DT + else + { + fprintf(stderr, "ERROR: CT_or_DT must be 'CT' or 'DT'\n"); + printf("instead got :%s\n", tmp_str); + return -1; + } + } + + else if (strcmp(tmp_str, "PID") == 0) + { + // pull out gains + if (json_object_object_get_ex(jobj_ctl, "kp", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find kp in settings file\n"); + return -1; + } + tmp_kp = json_object_get_double(tmp); + if (json_object_object_get_ex(jobj_ctl, "ki", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find ki in settings file\n"); + return -1; + } + tmp_ki = json_object_get_double(tmp); + if (json_object_object_get_ex(jobj_ctl, "kd", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find kd in settings file\n"); + return -1; + } + tmp_kd = json_object_get_double(tmp); + // get the crossover frequency + if (json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find crossover frequency in settings file\n"); + return -1; + } + if (json_object_is_type(tmp, json_type_double) == 0) + { + fprintf(stderr, "ERROR: crossover frequency should be a double\n"); + return -1; + } + tmp_flt = json_object_get_double(tmp); + if (rc_filter_pid(filter, tmp_kp, tmp_ki, tmp_kd, 1.0 / tmp_flt, DT)) + { + fprintf(stderr, "ERROR: failed to alloc pid filter in __parse_controller()"); + return -1; + } + } + +#ifdef DEBUG + rc_filter_print(*filter); +#endif + + rc_vector_free(&num_vec); + rc_vector_free(&den_vec); + + return 0; } - - - int settings_load_from_file(char* path) { - struct json_object *tmp = NULL; // temp object - char* tmp_str = NULL; // temp string poitner - double tmp_flt; - int tmp_int; - - was_load_successful = 0; - - #ifdef DEBUG - fprintf(stderr,"beginning of load_settings_from_file\n"); - fprintf(stderr,"about to check access of fly settings file\n"); - #endif - - // read in file contents - if(access(path, F_OK)!=0){ - fprintf(stderr,"ERROR: settings file missing\n"); - return -1; - } - else{ - #ifdef DEBUG - printf("about to read json from file\n"); - #endif - jobj = json_object_from_file(path); - if(jobj==NULL){ - fprintf(stderr,"ERROR, failed to read settings from disk\n"); - return -1; - } - } - - #ifdef DEBUG - settings_print(); - #endif - - // START PARSING - - PARSE_STRING(name) - #ifdef DEBUG - fprintf(stderr,"name: %s\n",settings.name); - #endif - PARSE_BOOL(warnings_en) - #ifdef DEBUG - fprintf(stderr,"warnings: %d\n",settings.warnings_en); - #endif - - - // PHYSICAL PARAMETERS - // layout populates num_rotors, layout, and dof - if(__parse_layout()==-1) return -1; // parse_layout also fill in num_rotors and dof - #ifdef DEBUG - fprintf(stderr,"layout:%d,%d\n",settings.layout,settings.num_rotors); - #endif - if(__parse_thrust_map()==-1) return -1; - #ifdef DEBUG - fprintf(stderr,"thrust_map: %d\n",settings.thrust_map); - #endif - PARSE_DOUBLE_MIN_MAX(v_nominal,7.0,18.0) - #ifdef DEBUG - fprintf(stderr,"v_nominal: %f\n",settings.v_nominal); - #endif - PARSE_BOOL(enable_magnetometer) - - - // FLIGHT MODES - PARSE_INT_MIN_MAX(num_dsm_modes,1,3) - if(json_object_object_get_ex(jobj, "flight_mode_1", &tmp)==0){ - fprintf(stderr,"ERROR: can't find flight_mode_1 in settings file\n"); - return -1; - } - if(__parse_flight_mode(tmp, &settings.flight_mode_1)) return -1; - #ifdef DEBUG - fprintf(stderr,"flight_mode_1: %d\n",settings.flight_mode_1); - #endif - if(json_object_object_get_ex(jobj, "flight_mode_2", &tmp)==0){ - fprintf(stderr,"ERROR: can't find flight_mode_2 in settings file\n"); - return -1; - } - if(__parse_flight_mode(tmp, &settings.flight_mode_2)) return -1; - #ifdef DEBUG - fprintf(stderr,"flight_mode_2: %d\n",settings.flight_mode_2); - #endif - if(json_object_object_get_ex(jobj, "flight_mode_3", &tmp)==0){ - fprintf(stderr,"ERROR: can't find flight_mode_3 in settings file\n"); - return -1; - } - if(__parse_flight_mode(tmp, &settings.flight_mode_3)) return -1; - #ifdef DEBUG - fprintf(stderr,"flight_mode_3: %d\n",settings.flight_mode_3); - #endif - - // DSM RADIO CONFIG - PARSE_INT_MIN_MAX(dsm_thr_ch,1,9) - PARSE_POLARITY(dsm_thr_pol) - PARSE_INT_MIN_MAX(dsm_roll_ch,1,9) - PARSE_POLARITY(dsm_roll_pol) - PARSE_INT_MIN_MAX(dsm_pitch_ch,1,9) - PARSE_POLARITY(dsm_pitch_pol) - PARSE_INT_MIN_MAX(dsm_yaw_ch,1,9) - PARSE_POLARITY(dsm_yaw_pol) - PARSE_INT_MIN_MAX(dsm_mode_ch,1,9) - PARSE_POLARITY(dsm_mode_pol) - if(__parse_kill_mode()==-1) return -1; - #ifdef DEBUG - fprintf(stderr,"kill_mode: %d\n",settings.dsm_kill_mode); - #endif - PARSE_INT_MIN_MAX(dsm_kill_ch,1,9) - PARSE_POLARITY(dsm_kill_pol) - - // PRINTF OPTIONS - PARSE_BOOL(printf_arm) - PARSE_BOOL(printf_altitude) - PARSE_BOOL(printf_rpy) - PARSE_BOOL(printf_sticks) - PARSE_BOOL(printf_setpoint) - PARSE_BOOL(printf_u) - PARSE_BOOL(printf_motors) - PARSE_BOOL(printf_mode) - - // LOGGING - PARSE_BOOL(enable_logging) - PARSE_BOOL(log_sensors) - PARSE_BOOL(log_state) - PARSE_BOOL(log_setpoint) - PARSE_BOOL(log_control_u) - PARSE_BOOL(log_motor_signals) - - // MAVLINK - PARSE_STRING(dest_ip) - PARSE_INT(my_sys_id) - PARSE_INT(mav_port) - - // FEEDBACK CONTROLLERS - PARSE_CONTROLLER(roll_controller) - PARSE_CONTROLLER(pitch_controller) - PARSE_CONTROLLER(yaw_controller) - PARSE_CONTROLLER(altitude_controller) - PARSE_CONTROLLER(horiz_vel_ctrl_4dof) - PARSE_CONTROLLER(horiz_vel_ctrl_6dof) - PARSE_CONTROLLER(horiz_pos_ctrl_4dof) - PARSE_CONTROLLER(horiz_pos_ctrl_6dof) - PARSE_DOUBLE_MIN_MAX(max_XY_velocity, .1, 10) - PARSE_DOUBLE_MIN_MAX(max_Z_velocity, .1, 10) - - json_object_put(jobj); // free memory - was_load_successful = 1; - return 0; + struct json_object* tmp = NULL; // temp object + char* tmp_str = NULL; // temp string poitner + double tmp_flt; + int tmp_int; + + was_load_successful = 0; + +#ifdef DEBUG + fprintf(stderr, "beginning of load_settings_from_file\n"); + fprintf(stderr, "about to check access of fly settings file\n"); +#endif + + // read in file contents + if (access(path, F_OK) != 0) + { + fprintf(stderr, "ERROR: settings file missing\n"); + return -1; + } + else + { +#ifdef DEBUG + printf("about to read json from file\n"); +#endif + jobj = json_object_from_file(path); + if (jobj == NULL) + { + fprintf(stderr, "ERROR, failed to read settings from disk\n"); + return -1; + } + } + +#ifdef DEBUG + settings_print(); +#endif + + // START PARSING + + PARSE_STRING(name) +#ifdef DEBUG + fprintf(stderr, "name: %s\n", settings.name); +#endif + PARSE_BOOL(warnings_en) +#ifdef DEBUG + fprintf(stderr, "warnings: %d\n", settings.warnings_en); +#endif + + // PHYSICAL PARAMETERS + // layout populates num_rotors, layout, and dof + if (__parse_layout() == -1) return -1; // parse_layout also fill in num_rotors and dof +#ifdef DEBUG + fprintf(stderr, "layout:%d,%d\n", settings.layout, settings.num_rotors); +#endif + if (__parse_thrust_map() == -1) return -1; +#ifdef DEBUG + fprintf(stderr, "thrust_map: %d\n", settings.thrust_map); +#endif + PARSE_DOUBLE_MIN_MAX(v_nominal, 7.0, 18.0) +#ifdef DEBUG + fprintf(stderr, "v_nominal: %f\n", settings.v_nominal); +#endif + PARSE_BOOL(enable_magnetometer) + + // FLIGHT MODES + PARSE_INT_MIN_MAX(num_dsm_modes, 1, 3) + if (json_object_object_get_ex(jobj, "flight_mode_1", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find flight_mode_1 in settings file\n"); + return -1; + } + if (__parse_flight_mode(tmp, &settings.flight_mode_1)) return -1; +#ifdef DEBUG + fprintf(stderr, "flight_mode_1: %d\n", settings.flight_mode_1); +#endif + if (json_object_object_get_ex(jobj, "flight_mode_2", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find flight_mode_2 in settings file\n"); + return -1; + } + if (__parse_flight_mode(tmp, &settings.flight_mode_2)) return -1; +#ifdef DEBUG + fprintf(stderr, "flight_mode_2: %d\n", settings.flight_mode_2); +#endif + if (json_object_object_get_ex(jobj, "flight_mode_3", &tmp) == 0) + { + fprintf(stderr, "ERROR: can't find flight_mode_3 in settings file\n"); + return -1; + } + if (__parse_flight_mode(tmp, &settings.flight_mode_3)) return -1; +#ifdef DEBUG + fprintf(stderr, "flight_mode_3: %d\n", settings.flight_mode_3); +#endif + + // DSM RADIO CONFIG + PARSE_INT_MIN_MAX(dsm_thr_ch, 1, 9) + PARSE_POLARITY(dsm_thr_pol) + PARSE_INT_MIN_MAX(dsm_roll_ch, 1, 9) + PARSE_POLARITY(dsm_roll_pol) + PARSE_INT_MIN_MAX(dsm_pitch_ch, 1, 9) + PARSE_POLARITY(dsm_pitch_pol) + PARSE_INT_MIN_MAX(dsm_yaw_ch, 1, 9) + PARSE_POLARITY(dsm_yaw_pol) + PARSE_INT_MIN_MAX(dsm_mode_ch, 1, 9) + PARSE_POLARITY(dsm_mode_pol) + if (__parse_kill_mode() == -1) return -1; +#ifdef DEBUG + fprintf(stderr, "kill_mode: %d\n", settings.dsm_kill_mode); +#endif + PARSE_INT_MIN_MAX(dsm_kill_ch, 1, 9) + PARSE_POLARITY(dsm_kill_pol) + + // PRINTF OPTIONS + PARSE_BOOL(printf_arm) + PARSE_BOOL(printf_altitude) + PARSE_BOOL(printf_rpy) + PARSE_BOOL(printf_sticks) + PARSE_BOOL(printf_setpoint) + PARSE_BOOL(printf_u) + PARSE_BOOL(printf_motors) + PARSE_BOOL(printf_mode) + + // LOGGING + PARSE_BOOL(enable_logging) + PARSE_BOOL(log_sensors) + PARSE_BOOL(log_state) + PARSE_BOOL(log_setpoint) + PARSE_BOOL(log_control_u) + PARSE_BOOL(log_motor_signals) + + // MAVLINK + PARSE_STRING(dest_ip) + PARSE_INT(my_sys_id) + PARSE_INT(mav_port) + + // FEEDBACK CONTROLLERS + PARSE_CONTROLLER(roll_controller) + PARSE_CONTROLLER(pitch_controller) + PARSE_CONTROLLER(yaw_controller) + PARSE_CONTROLLER(altitude_controller) + PARSE_CONTROLLER(horiz_vel_ctrl_4dof) + PARSE_CONTROLLER(horiz_vel_ctrl_6dof) + PARSE_CONTROLLER(horiz_pos_ctrl_4dof) + PARSE_CONTROLLER(horiz_pos_ctrl_6dof) + PARSE_DOUBLE_MIN_MAX(max_XY_velocity, .1, 10) + PARSE_DOUBLE_MIN_MAX(max_Z_velocity, .1, 10) + + json_object_put(jobj); // free memory + was_load_successful = 1; + return 0; } - - - int settings_print(void) { - if(jobj == NULL){ - fprintf(stderr,"ERROR: NULL object passed to settings_print\n"); - return -1; - } - printf("settings:\n\n"); - printf("%s", json_object_to_json_string_ext(jobj, \ - JSON_C_TO_STRING_SPACED | JSON_C_TO_STRING_PRETTY)); - printf("\n"); - return 0; + if (jobj == NULL) + { + fprintf(stderr, "ERROR: NULL object passed to settings_print\n"); + return -1; + } + printf("settings:\n\n"); + printf("%s", + json_object_to_json_string_ext(jobj, JSON_C_TO_STRING_SPACED | JSON_C_TO_STRING_PRETTY)); + printf("\n"); + return 0; } - diff --git a/src/state_estimator.c b/src/state_estimator.c index 2fbb94f..fb9caa5 100644 --- a/src/state_estimator.c +++ b/src/state_estimator.c @@ -3,27 +3,27 @@ * */ -#include #include +#include +#include +#include #include #include -#include #include #include -#include -#include +#include #include -#include +#include #include -#include +#include #include -#include #include +#include -#define TWO_PI (M_PI*2.0) +#define TWO_PI (M_PI * 2.0) -state_estimate_t state_estimate; // extern variable in state_estimator.h +state_estimate_t state_estimate; // extern variable in state_estimator.h // sensor data structs rc_mpu_data_t mpu_data; @@ -36,123 +36,123 @@ static rc_filter_t batt_lp = RC_FILTER_INITIALIZER; static rc_kalman_t alt_kf = RC_KALMAN_INITIALIZER; static rc_filter_t acc_lp = RC_FILTER_INITIALIZER; - static void __batt_init(void) { - // init the battery low pass filter - rc_filter_moving_average(&batt_lp, 20, DT); - double tmp = rc_adc_dc_jack(); - if(tmp<3.0){ - tmp = settings.v_nominal; - if(settings.warnings_en){ - fprintf(stderr, "WARNING: ADC read %0.1fV on the barrel jack. Please connect\n"); - fprintf(stderr, "battery to barrel jack, assuming nominal voltage for now.\n"); - } - } - rc_filter_prefill_inputs(&batt_lp, tmp); - rc_filter_prefill_outputs(&batt_lp, tmp); - return; + // init the battery low pass filter + rc_filter_moving_average(&batt_lp, 20, DT); + double tmp = rc_adc_dc_jack(); + if (tmp < 3.0) + { + tmp = settings.v_nominal; + if (settings.warnings_en) + { + fprintf(stderr, "WARNING: ADC read %0.1fV on the barrel jack. Please connect\n"); + fprintf(stderr, "battery to barrel jack, assuming nominal voltage for now.\n"); + } + } + rc_filter_prefill_inputs(&batt_lp, tmp); + rc_filter_prefill_outputs(&batt_lp, tmp); + return; } - static void __batt_march(void) { - double tmp = rc_adc_dc_jack(); - if(tmp<3.0) tmp = settings.v_nominal; - state_estimate.v_batt_raw = tmp; - state_estimate.v_batt_lp = rc_filter_march(&batt_lp, tmp); - return; + double tmp = rc_adc_dc_jack(); + if (tmp < 3.0) tmp = settings.v_nominal; + state_estimate.v_batt_raw = tmp; + state_estimate.v_batt_lp = rc_filter_march(&batt_lp, tmp); + return; } static void __batt_cleanup(void) { - rc_filter_free(&batt_lp); - return; + rc_filter_free(&batt_lp); + return; } - - static void __imu_march(void) { - static double last_yaw = 0.0; - static int num_yaw_spins = 0; - double diff; - - // gyro and accel require converting to NED coordinates - state_estimate.gyro[0] = mpu_data.gyro[1]; - state_estimate.gyro[1] = mpu_data.gyro[0]; - state_estimate.gyro[2] = -mpu_data.gyro[2]; - state_estimate.accel[0] = mpu_data.accel[1]; - state_estimate.accel[1] = mpu_data.accel[0]; - state_estimate.accel[2] = -mpu_data.accel[2]; - - // quaternion also needs coordinate transform - state_estimate.quat_imu[0] = mpu_data.dmp_quat[0]; // W - state_estimate.quat_imu[1] = mpu_data.dmp_quat[2]; // X (i) - state_estimate.quat_imu[2] = mpu_data.dmp_quat[1]; // Y (j) - state_estimate.quat_imu[3] = -mpu_data.dmp_quat[3]; // Z (k) - - // normalize it just in case - rc_quaternion_norm_array(state_estimate.quat_imu); - // generate tait bryan angles - rc_quaternion_to_tb_array(state_estimate.quat_imu, state_estimate.tb_imu); - - // yaw is more annoying since we have to detect spins - // also make sign negative since NED coordinates has Z point down - diff = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI) - last_yaw; - //detect the crossover point at +-PI and update num yaw spins - if(diff < -M_PI) num_yaw_spins++; - else if(diff > M_PI) num_yaw_spins--; - - // finally the new value can be written - state_estimate.imu_continuous_yaw = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI); - last_yaw = state_estimate.imu_continuous_yaw; - return; + static double last_yaw = 0.0; + static int num_yaw_spins = 0; + double diff; + + // gyro and accel require converting to NED coordinates + state_estimate.gyro[0] = mpu_data.gyro[1]; + state_estimate.gyro[1] = mpu_data.gyro[0]; + state_estimate.gyro[2] = -mpu_data.gyro[2]; + state_estimate.accel[0] = mpu_data.accel[1]; + state_estimate.accel[1] = mpu_data.accel[0]; + state_estimate.accel[2] = -mpu_data.accel[2]; + + // quaternion also needs coordinate transform + state_estimate.quat_imu[0] = mpu_data.dmp_quat[0]; // W + state_estimate.quat_imu[1] = mpu_data.dmp_quat[2]; // X (i) + state_estimate.quat_imu[2] = mpu_data.dmp_quat[1]; // Y (j) + state_estimate.quat_imu[3] = -mpu_data.dmp_quat[3]; // Z (k) + + // normalize it just in case + rc_quaternion_norm_array(state_estimate.quat_imu); + // generate tait bryan angles + rc_quaternion_to_tb_array(state_estimate.quat_imu, state_estimate.tb_imu); + + // yaw is more annoying since we have to detect spins + // also make sign negative since NED coordinates has Z point down + diff = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI) - last_yaw; + // detect the crossover point at +-PI and update num yaw spins + if (diff < -M_PI) + num_yaw_spins++; + else if (diff > M_PI) + num_yaw_spins--; + + // finally the new value can be written + state_estimate.imu_continuous_yaw = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI); + last_yaw = state_estimate.imu_continuous_yaw; + return; } - static void __mag_march(void) { - static double last_yaw = 0.0; - static int num_yaw_spins = 0; - - // don't do anything if mag isn't enabled - if(!settings.enable_magnetometer) return; - - // mag require converting to NED coordinates - state_estimate.mag[0] = mpu_data.mag[1]; - state_estimate.mag[1] = mpu_data.mag[0]; - state_estimate.mag[2] = -mpu_data.mag[2]; - - // quaternion also needs coordinate transform - state_estimate.quat_mag[0] = mpu_data.fused_quat[0]; // W - state_estimate.quat_mag[1] = mpu_data.fused_quat[2]; // X (i) - state_estimate.quat_mag[2] = mpu_data.fused_quat[1]; // Y (j) - state_estimate.quat_mag[3] = -mpu_data.fused_quat[3]; // Z (k) - - // normalize it just in case - rc_quaternion_norm_array(state_estimate.quat_mag); - // generate tait bryan angles - rc_quaternion_to_tb_array(state_estimate.quat_mag, state_estimate.tb_mag); - - // heading - state_estimate.mag_heading_raw = mpu_data.compass_heading_raw; - state_estimate.mag_heading = state_estimate.tb_mag[2]; - - // yaw is more annoying since we have to detect spins - // also make sign negative since NED coordinates has Z point down - double diff = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI) - last_yaw; - //detect the crossover point at +-PI and update num yaw spins - if(diff < -M_PI) num_yaw_spins++; - else if(diff > M_PI) num_yaw_spins--; - - // finally the new value can be written - state_estimate.mag_heading_continuous = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI); - last_yaw = state_estimate.mag_heading_continuous; - return; + static double last_yaw = 0.0; + static int num_yaw_spins = 0; + + // don't do anything if mag isn't enabled + if (!settings.enable_magnetometer) return; + + // mag require converting to NED coordinates + state_estimate.mag[0] = mpu_data.mag[1]; + state_estimate.mag[1] = mpu_data.mag[0]; + state_estimate.mag[2] = -mpu_data.mag[2]; + + // quaternion also needs coordinate transform + state_estimate.quat_mag[0] = mpu_data.fused_quat[0]; // W + state_estimate.quat_mag[1] = mpu_data.fused_quat[2]; // X (i) + state_estimate.quat_mag[2] = mpu_data.fused_quat[1]; // Y (j) + state_estimate.quat_mag[3] = -mpu_data.fused_quat[3]; // Z (k) + + // normalize it just in case + rc_quaternion_norm_array(state_estimate.quat_mag); + // generate tait bryan angles + rc_quaternion_to_tb_array(state_estimate.quat_mag, state_estimate.tb_mag); + + // heading + state_estimate.mag_heading_raw = mpu_data.compass_heading_raw; + state_estimate.mag_heading = state_estimate.tb_mag[2]; + + // yaw is more annoying since we have to detect spins + // also make sign negative since NED coordinates has Z point down + double diff = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI) - last_yaw; + // detect the crossover point at +-PI and update num yaw spins + if (diff < -M_PI) + num_yaw_spins++; + else if (diff > M_PI) + num_yaw_spins--; + + // finally the new value can be written + state_estimate.mag_heading_continuous = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI); + last_yaw = state_estimate.mag_heading_continuous; + return; } - /** * @brief initialize the altitude kalman filter * @@ -160,7 +160,7 @@ static void __mag_march(void) */ static int __altitude_init(void) { - //initialize altitude kalman filter and bmp sensor + // initialize altitude kalman filter and bmp sensor rc_matrix_t F = RC_MATRIX_INITIALIZER; rc_matrix_t G = RC_MATRIX_INITIALIZER; rc_matrix_t H = RC_MATRIX_INITIALIZER; @@ -168,198 +168,199 @@ static int __altitude_init(void) rc_matrix_t R = RC_MATRIX_INITIALIZER; rc_matrix_t Pi = RC_MATRIX_INITIALIZER; - const int Nx = 3; - const int Ny = 1; - const int Nu = 1; - - // allocate appropirate memory for system - rc_matrix_zeros(&F, Nx, Nx); - rc_matrix_zeros(&G, Nx, Nu); - rc_matrix_zeros(&H, Ny, Nx); - rc_matrix_zeros(&Q, Nx, Nx); - rc_matrix_zeros(&R, Ny, Ny); - rc_matrix_zeros(&Pi, Nx, Nx); - - // define system -DT; // accel bias - F.d[0][0] = 1.0; - F.d[0][1] = DT; - F.d[0][2] = 0.0; - F.d[1][0] = 0.0; - F.d[1][1] = 1.0; - F.d[1][2] = -DT; // subtract accel bias - F.d[2][0] = 0.0; - F.d[2][1] = 0.0; - F.d[2][2] = 1.0; // accel bias state - - G.d[0][0] = 0.5*DT*DT; - G.d[0][1] = DT; - G.d[0][2] = 0.0; - - H.d[0][0] = 1.0; - H.d[0][1] = 0.0; - H.d[0][2] = 0.0; - - // covariance matrices - Q.d[0][0] = 0.000000001; - Q.d[1][1] = 0.000000001; - Q.d[2][2] = 0.0001; // don't want bias to change too quickly - R.d[0][0] = 1000000.0; - - // initial P, cloned from converged P while running - Pi.d[0][0] = 1258.69; - Pi.d[0][1] = 158.6114; - Pi.d[0][2] = -9.9937; - Pi.d[1][0] = 158.6114; - Pi.d[1][1] = 29.9870; - Pi.d[1][2] = -2.5191; - Pi.d[2][0] = -9.9937; - Pi.d[2][1] = -2.5191; - Pi.d[2][2] = 0.3174; - - // initialize the kalman filter - if(rc_kalman_alloc_lin(&alt_kf,F,G,H,Q,R,Pi)==-1) return -1; - rc_matrix_free(&F); - rc_matrix_free(&G); - rc_matrix_free(&H); - rc_matrix_free(&Q); - rc_matrix_free(&R); - rc_matrix_free(&Pi); - - // initialize the little LP filter to take out accel noise - if(rc_filter_first_order_lowpass(&acc_lp, DT, 20*DT)) return -1; - - // init barometer and read in first data - if(rc_bmp_read(&bmp_data)) return -1; - - return 0; + const int Nx = 3; + const int Ny = 1; + const int Nu = 1; + + // allocate appropirate memory for system + rc_matrix_zeros(&F, Nx, Nx); + rc_matrix_zeros(&G, Nx, Nu); + rc_matrix_zeros(&H, Ny, Nx); + rc_matrix_zeros(&Q, Nx, Nx); + rc_matrix_zeros(&R, Ny, Ny); + rc_matrix_zeros(&Pi, Nx, Nx); + + // define system -DT; // accel bias + F.d[0][0] = 1.0; + F.d[0][1] = DT; + F.d[0][2] = 0.0; + F.d[1][0] = 0.0; + F.d[1][1] = 1.0; + F.d[1][2] = -DT; // subtract accel bias + F.d[2][0] = 0.0; + F.d[2][1] = 0.0; + F.d[2][2] = 1.0; // accel bias state + + G.d[0][0] = 0.5 * DT * DT; + G.d[0][1] = DT; + G.d[0][2] = 0.0; + + H.d[0][0] = 1.0; + H.d[0][1] = 0.0; + H.d[0][2] = 0.0; + + // covariance matrices + Q.d[0][0] = 0.000000001; + Q.d[1][1] = 0.000000001; + Q.d[2][2] = 0.0001; // don't want bias to change too quickly + R.d[0][0] = 1000000.0; + + // initial P, cloned from converged P while running + Pi.d[0][0] = 1258.69; + Pi.d[0][1] = 158.6114; + Pi.d[0][2] = -9.9937; + Pi.d[1][0] = 158.6114; + Pi.d[1][1] = 29.9870; + Pi.d[1][2] = -2.5191; + Pi.d[2][0] = -9.9937; + Pi.d[2][1] = -2.5191; + Pi.d[2][2] = 0.3174; + + // initialize the kalman filter + if (rc_kalman_alloc_lin(&alt_kf, F, G, H, Q, R, Pi) == -1) return -1; + rc_matrix_free(&F); + rc_matrix_free(&G); + rc_matrix_free(&H); + rc_matrix_free(&Q); + rc_matrix_free(&R); + rc_matrix_free(&Pi); + + // initialize the little LP filter to take out accel noise + if (rc_filter_first_order_lowpass(&acc_lp, DT, 20 * DT)) return -1; + + // init barometer and read in first data + if (rc_bmp_read(&bmp_data)) return -1; + + return 0; } static void __altitude_march(void) { - int i; - double accel_vec[3]; - static rc_vector_t u = RC_VECTOR_INITIALIZER; - static rc_vector_t y = RC_VECTOR_INITIALIZER; + int i; + double accel_vec[3]; + static rc_vector_t u = RC_VECTOR_INITIALIZER; + static rc_vector_t y = RC_VECTOR_INITIALIZER; - // grab raw data - state_estimate.bmp_pressure_raw = bmp_data.pressure_pa; - state_estimate.alt_bmp_raw = bmp_data.alt_m; - state_estimate.bmp_temp = bmp_data.temp_c; + // grab raw data + state_estimate.bmp_pressure_raw = bmp_data.pressure_pa; + state_estimate.alt_bmp_raw = bmp_data.alt_m; + state_estimate.bmp_temp = bmp_data.temp_c; - // make copy of acceleration reading before rotating - for(i=0;i<3;i++) accel_vec[i] = state_estimate.accel[i]; + // make copy of acceleration reading before rotating + for (i = 0; i < 3; i++) accel_vec[i] = state_estimate.accel[i]; - // rotate accel vector - rc_quaternion_rotate_vector_array(accel_vec, state_estimate.quat_imu); + // rotate accel vector + rc_quaternion_rotate_vector_array(accel_vec, state_estimate.quat_imu); - // do first-run filter setup - if(alt_kf.step==0){ + // do first-run filter setup + if (alt_kf.step == 0) + { rc_vector_zeros(&u, 1); rc_vector_zeros(&y, 1); - alt_kf.x_est.d[0] = -bmp_data.alt_m; - rc_filter_prefill_inputs(&acc_lp, accel_vec[2]+GRAVITY); - rc_filter_prefill_outputs(&acc_lp, accel_vec[2]+GRAVITY); - } + alt_kf.x_est.d[0] = -bmp_data.alt_m; + rc_filter_prefill_inputs(&acc_lp, accel_vec[2] + GRAVITY); + rc_filter_prefill_outputs(&acc_lp, accel_vec[2] + GRAVITY); + } - // calculate acceleration and smooth it just a tad - // put result in u for kalman and flip sign since with altitude, positive - // is up whereas acceleration in Z points down. - rc_filter_march(&acc_lp, accel_vec[2]+GRAVITY); - u.d[0] = acc_lp.newest_output; + // calculate acceleration and smooth it just a tad + // put result in u for kalman and flip sign since with altitude, positive + // is up whereas acceleration in Z points down. + rc_filter_march(&acc_lp, accel_vec[2] + GRAVITY); + u.d[0] = acc_lp.newest_output; - // don't bother filtering Barometer, kalman will deal with that - y.d[0] = -bmp_data.alt_m; + // don't bother filtering Barometer, kalman will deal with that + y.d[0] = -bmp_data.alt_m; - rc_kalman_update_lin(&alt_kf, u, y); + rc_kalman_update_lin(&alt_kf, u, y); - // altitude estimate - state_estimate.alt_bmp = alt_kf.x_est.d[0]; - state_estimate.alt_bmp_vel = alt_kf.x_est.d[1]; - state_estimate.alt_bmp_accel= alt_kf.x_est.d[2]; + // altitude estimate + state_estimate.alt_bmp = alt_kf.x_est.d[0]; + state_estimate.alt_bmp_vel = alt_kf.x_est.d[1]; + state_estimate.alt_bmp_accel = alt_kf.x_est.d[2]; - return; + return; } static void __feedback_select(void) { - state_estimate.roll = state_estimate.tb_imu[0]; - state_estimate.pitch = state_estimate.tb_imu[1]; - state_estimate.yaw = state_estimate.tb_imu[2]; - state_estimate.continuous_yaw = state_estimate.imu_continuous_yaw; - state_estimate.X = state_estimate.pos_mocap[0]; - state_estimate.Y = state_estimate.pos_mocap[1]; - state_estimate.Z = state_estimate.alt_bmp; + state_estimate.roll = state_estimate.tb_imu[0]; + state_estimate.pitch = state_estimate.tb_imu[1]; + state_estimate.yaw = state_estimate.tb_imu[2]; + state_estimate.continuous_yaw = state_estimate.imu_continuous_yaw; + state_estimate.X = state_estimate.pos_mocap[0]; + state_estimate.Y = state_estimate.pos_mocap[1]; + state_estimate.Z = state_estimate.alt_bmp; } static void __altitude_cleanup(void) { - rc_kalman_free(&alt_kf); - rc_filter_free(&acc_lp); - return; + rc_kalman_free(&alt_kf); + rc_filter_free(&acc_lp); + return; } - - static void __mocap_check_timeout(void) { - if(state_estimate.mocap_running){ - uint64_t current_time = rc_nanos_since_boot(); - // check if mocap data is > 3 steps old - if((current_time-state_estimate.mocap_timestamp_ns) > (3*1E7)){ - state_estimate.mocap_running = 0; - if(settings.warnings_en){ - fprintf(stderr,"WARNING, MOCAP LOST VISUAL\n"); - } - } - } - return; + if (state_estimate.mocap_running) + { + uint64_t current_time = rc_nanos_since_boot(); + // check if mocap data is > 3 steps old + if ((current_time - state_estimate.mocap_timestamp_ns) > (3 * 1E7)) + { + state_estimate.mocap_running = 0; + if (settings.warnings_en) + { + fprintf(stderr, "WARNING, MOCAP LOST VISUAL\n"); + } + } + } + return; } - int state_estimator_init(void) { - __batt_init(); - if(__altitude_init()) return -1; - state_estimate.initialized = 1; - return 0; + __batt_init(); + if (__altitude_init()) return -1; + state_estimate.initialized = 1; + return 0; } int state_estimator_march(void) { - if(state_estimate.initialized==0){ - fprintf(stderr, "ERROR in state_estimator_march, estimator not initialized\n"); - return -1; - } - - // populate state_estimate struct one setion at a time, top to bottom - __batt_march(); - __imu_march(); - __mag_march(); - __altitude_march(); - __feedback_select(); - __mocap_check_timeout(); - return 0; + if (state_estimate.initialized == 0) + { + fprintf(stderr, "ERROR in state_estimator_march, estimator not initialized\n"); + return -1; + } + + // populate state_estimate struct one setion at a time, top to bottom + __batt_march(); + __imu_march(); + __mag_march(); + __altitude_march(); + __feedback_select(); + __mocap_check_timeout(); + return 0; } - int state_estimator_jobs_after_feedback(void) { - static int bmp_sample_counter = 0; - - // check if we need to sample BMP this loop - if(bmp_sample_counter>=BMP_RATE_DIV){ - // perform the i2c reads to the sensor, on bad read just try later - if(rc_bmp_read(&bmp_data)) return -1; - bmp_sample_counter=0; - } - bmp_sample_counter++; - return 0; + static int bmp_sample_counter = 0; + + // check if we need to sample BMP this loop + if (bmp_sample_counter >= BMP_RATE_DIV) + { + // perform the i2c reads to the sensor, on bad read just try later + if (rc_bmp_read(&bmp_data)) return -1; + bmp_sample_counter = 0; + } + bmp_sample_counter++; + return 0; } - int state_estimator_cleanup(void) { - __batt_cleanup(); - __altitude_cleanup(); - return 0; + __batt_cleanup(); + __altitude_cleanup(); + return 0; } \ No newline at end of file diff --git a/src/thrust_map.c b/src/thrust_map.c index e7ff354..fbe45df 100644 --- a/src/thrust_map.c +++ b/src/thrust_map.c @@ -16,67 +16,66 @@ static double* signal; static double* thrust; static int points; +// clang-format off + // Generic linear mapping static const int linear_map_points = 11; -static double linear_map[][2] = \ -{{0.0, 0.0000}, \ - {0.1, 0.1000}, \ - {0.2, 0.2000}, \ - {0.3, 0.3000}, \ - {0.4, 0.4000}, \ - {0.5, 0.5000}, \ - {0.6, 0.6000}, \ - {0.7, 0.7000}, \ - {0.8, 0.8000}, \ - {0.9, 0.9000}, \ - {1.0, 1.0000}}; +static double linear_map[][2] = { + {0.0, 0.0000}, + {0.1, 0.1000}, + {0.2, 0.2000}, + {0.3, 0.3000}, + {0.4, 0.4000}, + {0.5, 0.5000}, + {0.6, 0.6000}, + {0.7, 0.7000}, + {0.8, 0.8000}, + {0.9, 0.9000}, + {1.0, 1.0000}}; // Tiger Motor MN1806, 1400KV 6x4.5" 3-blade prop, 14.8V, // BLheli ESC Low Timing // this one is in Newtons but it doesn't really matter static const int mn1806_1400kv_4s_points = 11; -static double mn1806_1400kv_4s_map[][2] = \ -{{0.0, 0.0000}, \ - {0.1, 0.2982}, \ - {0.2, 0.6310}, \ - {0.3, 1.0281}, \ - {0.4, 1.5224}, \ - {0.5, 2.0310}, \ - {0.6, 2.5791}, \ - {0.7, 3.1365}, \ - {0.8, 3.7282}, \ - {0.9, 4.3147}, \ - {1.0, 4.7258}}; - +static double mn1806_1400kv_4s_map[][2] = { + {0.0, 0.0000}, + {0.1, 0.2982}, + {0.2, 0.6310}, + {0.3, 1.0281}, + {0.4, 1.5224}, + {0.5, 2.0310}, + {0.6, 2.5791}, + {0.7, 3.1365}, + {0.8, 3.7282}, + {0.9, 4.3147}, + {1.0, 4.7258}}; // tiger motor F20 2300kv motor, 2S lipo, 4x4.0" 3-blade props // blheli esc med-low timing // thrust units in gram-force but doesn't really matter static const int f20_2300kv_2s_points = 21; -static double f20_2300kv_2s_map[][2] = \ -{{0.00, 0.000000}, \ - {0.05, 6.892067}, \ - {0.10, 12.57954}, \ - {0.15, 18.84790}, \ - {0.20, 26.16294}, \ - {0.25, 33.98255}, \ - {0.30, 41.60790}, \ - {0.35, 49.32732}, \ - {0.40, 58.27048}, \ - {0.45, 67.83613}, \ - {0.50, 78.20817}, \ - {0.55, 88.27728}, \ - {0.60, 100.1058}, \ - {0.65, 110.3643}, \ - {0.70, 121.6316}, \ - {0.75, 132.2155}, \ - {0.80, 145.0420}, \ - {0.85, 154.6838}, \ - {0.90, 162.0185}, \ - {0.95, 168.4321}, \ - {1.00, 177.1643}}; - - +static double f20_2300kv_2s_map[][2] = { + {0.00, 0.000000}, + {0.05, 6.892067}, + {0.10, 12.57954}, + {0.15, 18.84790}, + {0.20, 26.16294}, + {0.25, 33.98255}, + {0.30, 41.60790}, + {0.35, 49.32732}, + {0.40, 58.27048}, + {0.45, 67.83613}, + {0.50, 78.20817}, + {0.55, 88.27728}, + {0.60, 100.1058}, + {0.65, 110.3643}, + {0.70, 121.6316}, + {0.75, 132.2155}, + {0.80, 145.0420}, + {0.85, 154.6838}, + {0.90, 162.0185}, + {0.95, 168.4321}, + {1.00, 177.1643}}; /* * Lumenier RX2206-13 2000kv motor, 4S lipo, 5x45" lumenier prop @@ -84,113 +83,126 @@ static double f20_2300kv_2s_map[][2] = \ * for 5" monocoque hex */ static const int rx2206_4s_points = 12; -static double rx2206_4s_map[][2] = \ -{{0.0 , 0.00000000000000}, \ - {0.05 , 17.8844719758775}, \ - {0.145 , 44.8761484808831}, \ - {0.24 , 80.0271164157384}, \ - {0.335 , 122.556484678150}, \ - {0.43 , 168.358712108506}, \ - {0.525 , 220.433636910433}, \ - {0.62 , 277.201919870206}, \ - {0.715 , 339.008615108196}, \ - {0.81 , 418.819295994349}, \ - {0.905 , 505.430124336786}, \ - {1.0 , 566.758535098236}}; - +static double rx2206_4s_map[][2] = { + {0.0, 0.00000000000000}, + {0.05, 17.8844719758775}, + {0.145, 44.8761484808831}, + {0.24, 80.0271164157384}, + {0.335, 122.556484678150}, + {0.43, 168.358712108506}, + {0.525, 220.433636910433}, + {0.62, 277.201919870206}, + {0.715, 339.008615108196}, + {0.81, 418.819295994349}, + {0.905, 505.430124336786}, + {1.0, 566.758535098236}}; + +// clang-format on int thrust_map_init(thrust_map_t map) { - int i; - double max; - double (*data)[2]; // pointer to constant data - - switch(map){ - case LINEAR_MAP: - points = linear_map_points; - data = linear_map; - break; - case MN1806_1400KV_4S: - points = mn1806_1400kv_4s_points; - data = mn1806_1400kv_4s_map; - break; - case F20_2300KV_2S: - points = f20_2300kv_2s_points; - data = f20_2300kv_2s_map; - break; - case RX2206_4S: - points = rx2206_4s_points; - data = rx2206_4s_map; - break; - default: - fprintf(stderr,"ERROR: unknown thrust map\n"); - return -1; - } - - // sanity checks - if(points<2){ - fprintf(stderr,"ERROR: need at least 2 datapoints in THRUST_MAP\n"); - return -1; - } - if(data[0][0] != 0.0){ - fprintf(stderr,"ERROR: first row input must be 0.0\n"); - return -1; - } - if(data[points-1][0] != 1.0){ - fprintf(stderr,"ERROR: last row input must be 1.0\n"); - printf("data: %f\n",data[points-1][0]); - return -1; - } - if(data[0][1] != 0.0){ - fprintf(stderr,"ERROR: first row thrust must be 0.0\n"); - return -1; - } - if(data[points-1][1] < 0.0){ - fprintf(stderr,"ERROR: last row thrust must be > 0.0\n"); - return -1; - } - for(i=1;i 0.0\n"); + return -1; + } + for (i = 1; i < points; i++) + { + if (data[i][0] <= data[i - 1][0] || data[i][1] <= data[i - 1][1]) + { + fprintf(stderr, "ERROR: thrust_map must be monotonically increasing\n"); + return -1; + } + } + + // create new global array of normalized thrust and inputs + if (signal != NULL) free(signal); + if (thrust != NULL) free(thrust); + signal = (double*)malloc(points * sizeof(double)); + thrust = (double*)malloc(points * sizeof(double)); + max = data[points - 1][1]; + for (i = 0; i < points; i++) + { + signal[i] = data[i][0]; + thrust[i] = data[i][1] / max; + } + return 0; } - -double map_motor_signal(double m){ - int i; - double pos; - - // sanity check - if(m>1.0 || m<0.0){ - printf("ERROR: desired thrust t must be between 0.0 & 1.0\n"); - return -1; - } - - // return quickly for boundary conditions - if(m==0.0 || m==1.0) return m; - - // scan through the data to pick the upper and lower points to interpolate - for(i=1; i 1.0 || m < 0.0) + { + printf("ERROR: desired thrust t must be between 0.0 & 1.0\n"); + return -1; + } + + // return quickly for boundary conditions + if (m == 0.0 || m == 1.0) return m; + + // scan through the data to pick the upper and lower points to interpolate + for (i = 1; i < points; i++) + { + if (m <= thrust[i]) + { + pos = (m - thrust[i - 1]) / (thrust[i] - thrust[i - 1]); + return signal[i - 1] + (pos * (signal[i] - signal[i - 1])); + } + } + + fprintf(stderr, "ERROR: something in map_motor_signal went wrong\n"); + return -1; } From e8fed6e58a4932ec75a6cac5900d056df6ea569c Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Mon, 19 Aug 2019 08:19:42 -0400 Subject: [PATCH 3/9] Added code and log transfer scripts --- scripts/copy_logs | 30 ++++++++++++++++++++++++++ scripts/transfer_rcpilot | 46 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100755 scripts/copy_logs create mode 100755 scripts/transfer_rcpilot diff --git a/scripts/copy_logs b/scripts/copy_logs new file mode 100755 index 0000000..02514e8 --- /dev/null +++ b/scripts/copy_logs @@ -0,0 +1,30 @@ +#!/bin/bash + +main(){ + if [ $# -eq 1 ]; then + if [ $1 = usb ]; then + LOC=debian@192.168.6.2 + elif [ $1 = wifi ]; then + LOC=debian@192.168.8.1 + else + usage_error + fi + else + usage_error + fi + + SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" + LOG_FILE=$SCRIPT_DIR/../../flight_logs/flight_logs_$(date +%F_%H%M%S) + + mkdir -p $LOG_FILE + + rsync -avzh --progress $LOC:/home/debian/rc_pilot_logs/ $LOG_FILE +} + +usage_error(){ + echo "ERROR: Please specify beaglebone connection" + echo "Usage: ./transfer_rcpilot [usb|wifi]" + exit 1 +} + +main "$@" \ No newline at end of file diff --git a/scripts/transfer_rcpilot b/scripts/transfer_rcpilot new file mode 100755 index 0000000..237ecfc --- /dev/null +++ b/scripts/transfer_rcpilot @@ -0,0 +1,46 @@ +#!/bin/bash + +main() { + # Parse input to determine the destination IP + if [ $# -eq 1 ]; then + if [ $1 = usb ]; then + DEST=debian@192.168.6.2 + elif [ $1 = wifi ]; then + DEST=debian@192.168.8.1 + else + usage_error + fi + else + usage_error + fi + + SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" + RC_PILOT_DIR=$SCRIPT_DIR/.. + BBB_DEST=$DEST:/home/debian/rc_pilot/ + TMP_FILE=/tmp/include_files + + # Folders and files to transfer to the beaglebone + INCLUDE_FILES='include/ src/ Makefile settings/' + + # Populates tmp_file with all of the files to send to the beaglebone + # (needed by the --include-from rsync flag) + find $INCLUDE_FILES > $TMP_FILE + + # Set the date on the beaglebone to mach source computer, eliminates clock skew on make + DATE=$(date) + SET_DATE="echo 'temppwd' | sudo -S date -s ""'""$DATE""'" + ssh -t $DEST $SET_DATE + + # Transfer the files to the beaglebone. Performs archiving operation that only updates files + # that have newer changes on the tranfering computer. Requires the date to be set properly for this + # to work. + rsync -avzh --include-from $TMP_FILE --exclude '*' --progress $RC_PILOT_DIR $BBB_DEST +} + +function usage_error() { + echo "ERROR: Please specify beaglebone connection" + echo "Usage: ./transfer_rcpilot [usb|wifi|a2sys]" + exit 1 +} + +main "$@" \ No newline at end of file From 94af7df0efefe93149835900c4cd09d3aebf3109 Mon Sep 17 00:00:00 2001 From: Glen Haggin <38386500+ghaggin@users.noreply.github.com> Date: Mon, 19 Aug 2019 08:50:54 -0400 Subject: [PATCH 4/9] Update transfer_rcpilot --- scripts/transfer_rcpilot | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/transfer_rcpilot b/scripts/transfer_rcpilot index 237ecfc..67909cb 100755 --- a/scripts/transfer_rcpilot +++ b/scripts/transfer_rcpilot @@ -39,8 +39,8 @@ main() { function usage_error() { echo "ERROR: Please specify beaglebone connection" - echo "Usage: ./transfer_rcpilot [usb|wifi|a2sys]" + echo "Usage: ./transfer_rcpilot [usb|wifi]" exit 1 } -main "$@" \ No newline at end of file +main "$@" From 7549fe8152945a172275510201a8f9e55c121c58 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Mon, 19 Aug 2019 08:53:17 -0400 Subject: [PATCH 5/9] Fixed all warnings --- .gitignore | 4 +- Makefile | 14 +- settings/jellyfish_settings.json | 228 +++++++++++++++++++++++++++++++ src/feedback.c | 5 +- src/input_manager.c | 2 +- src/log_manager.c | 4 +- src/mavlink_manager.c | 5 - src/printf_manager.c | 7 +- src/settings.c | 5 - src/state_estimator.c | 3 +- 10 files changed, 247 insertions(+), 30 deletions(-) create mode 100644 settings/jellyfish_settings.json diff --git a/.gitignore b/.gitignore index 1d0e46a..394a484 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ -docs/html/* +bin/ +build/ +docs/html/ .vscode/ \ No newline at end of file diff --git a/Makefile b/Makefile index 5d16ee1..31637de 100644 --- a/Makefile +++ b/Makefile @@ -1,25 +1,23 @@ -# This is a general use makefile for robotics cape projects written in C. -# Just change the target name to match your main source code filename. - +# Directory and file variables SRCDIR := src BINDIR := bin BUILDDIR := build INCLUDEDIR := include TARGET := $(BINDIR)/rc_pilot -# file definitions for rules +# File definitions for rules SOURCES := $(shell find $(SRCDIR) -type f -name *.c) OBJECTS := $(SOURCES:$(SRCDIR)/%.c=$(BUILDDIR)/%.o) INCLUDES := $(shell find $(INCLUDEDIR) -name '*.h') -CC := gcc +CC := gcc LINKER := gcc -WFLAGS := -Wall -Wextra +WFLAGS := -Wall -Wextra -Werror CFLAGS := -I $(INCLUDEDIR) OPT_FLAGS := -O1 LDFLAGS := -lm -lrt -pthread -lrobotcontrol -ljson-c -RM := rm -rf +RM := rm -rf INSTALL := install -m 4755 INSTALLDIR := install -d -m 755 @@ -39,7 +37,7 @@ $(TARGET): $(OBJECTS) # rule for all other objects $(BUILDDIR)/%.o : $(SRCDIR)/%.c $(INCLUDES) @mkdir -p $(dir $(@)) - @$(CC) -c $(CFLAGS) $(OPT_FLAGS) $(DEBUGFLAG) $< -o $(@) + @$(CC) -c $(CFLAGS) $(OPT_FLAGS) $(DEBUGFLAG) $(WFLAGS) $< -o $(@) @echo "made: $(@)" all: $(TARGET) diff --git a/settings/jellyfish_settings.json b/settings/jellyfish_settings.json new file mode 100644 index 0000000..6f7a7e9 --- /dev/null +++ b/settings/jellyfish_settings.json @@ -0,0 +1,228 @@ +{ + "name": "QUADCOPTER", + + "warnings_en": true, + + "layout": "LAYOUT_4X", + "thrust_map": "AIR2213_3S", + "orientation": "ORIENTATION_X_FORWARD", + "v_nominal": 12.5, + + "enable_magnetometer": true, + + "num_dsm_modes": 2, + "flight_mode_1": "TEST_BENCH_4DOF", + "flight_mode_2": "AUTONOMOUS", + "flight_mode_3": "DIRECT_THROTTLE_4DOF", + + "dsm_thr_ch": 1, + "dsm_thr_pol": 1, + "dsm_roll_ch": 2, + "dsm_roll_pol": 1, + "dsm_pitch_ch": 3, + "dsm_pitch_pol": 1, + "dsm_yaw_ch": 4, + "dsm_yaw_pol": 1, + "dsm_mode_ch": 5, + "dsm_mode_pol": 1, + "dsm_kill_mode": "DSM_KILL_DEDICATED_SWITCH", + "dsm_kill_ch": 6, + "dsm_kill_pol": 1, + + "printf_arm": true, + "printf_altitude": true, + "printf_battery": true, + "printf_position": true, + "printf_rpy": true, + "printf_sticks": true, + "printf_setpoint": false, + "printf_u": false, + "printf_motors": false, + "printf_mode": true, + "printf_xbee": false, + "printf_tracking": true, + + "enable_logging": true, + "log_sensors": true, + "log_state": true, + "log_xbee": true, + "log_gps": true, + "log_setpoint": true, + "log_control_u": true, + "log_motor_signals": true, + "log_throttles": true, + "log_dsm": true, + "log_flight_mode": true, + + "dest_ip": "192.168.8.1", + "my_sys_id": 1, + "mav_port": 14551, + + "en_sp_filtering": true, + "sp_filter_alpha": 0.9, + + "dsm_timeout_ms": 200, + + "roll_controller": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.5, + "ki":0.0, + "kd":0.1, + "crossover_freq_rad_per_sec": 62.83, + "numerator": [ + 0.1, + 0.1, + 0.1 + ], + "denominator": [ + 0.1, + 0.1, + 0.1 + ] + }, + + "pitch_controller": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.4, + "ki":0.0, + "kd":0.1, + "crossover_freq_rad_per_sec": 62.83, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "yaw_controller": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.5947, + "ki":0.0, + "kd":1.0, + "crossover_freq_rad_per_sec": 31.41, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "altitude_controller": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.1616, + "ki":0.0, + "kd":1.154, + "crossover_freq_rad_per_sec": 62.83, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "horiz_vel_ctrl_4dof": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.85, + "ki":0.0, + "kd":0.0, + "crossover_freq_rad_per_sec": 6.283, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "horiz_vel_ctrl_6dof": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.85, + "ki":0.0, + "kd":0.0, + "crossover_freq_rad_per_sec": 6.283, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "horiz_pos_ctrl_4dof": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":1.2, + "ki":0.0, + "kd":0.1, + "crossover_freq_rad_per_sec": 62.83, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "horiz_pos_ctrl_6dof": { + "gain": 1.0, + "CT_or_DT": "CT", + "TF_or_PID": "PID", + "kp":0.85, + "ki":0.0, + "kd":0.0, + "crossover_freq_rad_per_sec": 6.283, + "numerator": [ + 0.1, + 0.2, + 0.3 + ], + "denominator": [ + 0.1, + 0.2, + 0.3 + ] + }, + + "max_XY_velocity": 1.0, + "max_Z_velocity": 1.0 +} \ No newline at end of file diff --git a/src/feedback.c b/src/feedback.c index d0dca89..a57488e 100644 --- a/src/feedback.c +++ b/src/feedback.c @@ -145,8 +145,6 @@ int feedback_arm(void) int feedback_init(void) { - double tmp; - __rpy_init(); rc_filter_duplicate(&D_Z, settings.altitude_controller); @@ -180,7 +178,6 @@ int feedback_march(void) int i; double tmp, min, max; double u[6], mot[8]; - log_entry_t new_log; static int last_en_Z_ctrl = 0; // Disarm if rc_state is somehow paused without disarming the controller. @@ -219,7 +216,7 @@ int feedback_march(void) ***************************************************************************/ // run altitude controller if enabled // this needs work... - // we need to: + // we need to // find hover thrust and correct from there // this code does not work a.t.m. if (setpoint.en_Z_ctrl) diff --git a/src/input_manager.c b/src/input_manager.c index d5667d8..ac76fa1 100644 --- a/src/input_manager.c +++ b/src/input_manager.c @@ -217,7 +217,7 @@ void dsm_disconnect_callback(void) fprintf(stderr, "LOST DSM CONNECTION\n"); } -void* input_manager(void* ptr) +void* input_manager(__attribute__((unused)) void* ptr) { user_input.initialized = 1; // wait for first packet diff --git a/src/log_manager.c b/src/log_manager.c index a2a8fb1..a7848f1 100644 --- a/src/log_manager.c +++ b/src/log_manager.c @@ -109,8 +109,8 @@ static int __write_log_entry(FILE* fd, log_entry_t e) if (settings.log_control_u) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.u_roll, e.u_pitch, e.u_yaw, - e.u_X, e.u_Y, e.u_Z); + fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.u_roll, e.u_pitch, e.u_yaw, e.u_X, e.u_Y, + e.u_Z); } if (settings.log_motor_signals && settings.num_rotors == 8) diff --git a/src/mavlink_manager.c b/src/mavlink_manager.c index b6ade89..4052c6d 100644 --- a/src/mavlink_manager.c +++ b/src/mavlink_manager.c @@ -64,11 +64,6 @@ static void __callback_func_mocap(void) int mavlink_manager_init(void) { - // set default options before checking options - const char* dest_ip = LOCALHOST_IP; - uint8_t my_sys_id = DEFAULT_SYS_ID; - uint16_t port = RC_MAV_DEFAULT_UDP_PORT; - // initialize the UDP port and listening thread with the rc_mav lib if (rc_mav_init(settings.my_sys_id, settings.dest_ip, settings.mav_port, RC_MAV_DEFAULT_CONNECTION_TIMEOUT_US) < 0) diff --git a/src/printf_manager.c b/src/printf_manager.c index 284c8ea..4854c82 100644 --- a/src/printf_manager.c +++ b/src/printf_manager.c @@ -120,9 +120,10 @@ static void* __printf_manager_func(__attribute__((unused)) void* ptr) while (rc_get_state() != EXITING) { // re-print header on disarming - // if(fstate.arm_state==DISARMED && prev_arm_state==ARMED){ - // __print_header(); - //} + if (fstate.arm_state == DISARMED && prev_arm_state == ARMED) + { + __print_header(); + } printf("\r"); if (settings.printf_arm) diff --git a/src/settings.c b/src/settings.c index 0a3a7bc..d1a4673 100644 --- a/src/settings.c +++ b/src/settings.c @@ -248,7 +248,6 @@ static int __parse_thrust_map(void) static int __parse_flight_mode(json_object* jobj_str, flight_mode_t* mode) { char* tmp_str = NULL; - struct json_object* tmp = NULL; if (json_object_is_type(jobj_str, json_type_string) == 0) { fprintf(stderr, "ERROR: flight_mode should be a string\n"); @@ -559,10 +558,6 @@ static int __parse_controller(json_object* jobj_ctl, rc_filter_t* filter) int settings_load_from_file(char* path) { struct json_object* tmp = NULL; // temp object - char* tmp_str = NULL; // temp string poitner - double tmp_flt; - int tmp_int; - was_load_successful = 0; #ifdef DEBUG diff --git a/src/state_estimator.c b/src/state_estimator.c index fb9caa5..d685dec 100644 --- a/src/state_estimator.c +++ b/src/state_estimator.c @@ -46,7 +46,8 @@ static void __batt_init(void) tmp = settings.v_nominal; if (settings.warnings_en) { - fprintf(stderr, "WARNING: ADC read %0.1fV on the barrel jack. Please connect\n"); + fprintf(stderr, "WARNING: ADC read %0.1fV on the barrel jack. Please connect\n", + state_estimate.v_batt_lp); fprintf(stderr, "battery to barrel jack, assuming nominal voltage for now.\n"); } } From 817484766aefd89f29ee9901eaa068b98d0c29e9 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Mon, 19 Aug 2019 09:05:33 -0400 Subject: [PATCH 6/9] Added thrust map for air 2213 motors --- include/thrust_map.h | 3 ++- settings/jellyfish_settings.json | 2 +- src/settings.c | 4 ++++ src/thrust_map.c | 18 ++++++++++++++++++ 4 files changed, 25 insertions(+), 2 deletions(-) diff --git a/include/thrust_map.h b/include/thrust_map.h index 402b49d..602aa3d 100644 --- a/include/thrust_map.h +++ b/include/thrust_map.h @@ -18,7 +18,8 @@ typedef enum thrust_map_t LINEAR_MAP, MN1806_1400KV_4S, F20_2300KV_2S, - RX2206_4S + RX2206_4S, + AIR2213_3S } thrust_map_t; /** diff --git a/settings/jellyfish_settings.json b/settings/jellyfish_settings.json index 6f7a7e9..537688b 100644 --- a/settings/jellyfish_settings.json +++ b/settings/jellyfish_settings.json @@ -12,7 +12,7 @@ "num_dsm_modes": 2, "flight_mode_1": "TEST_BENCH_4DOF", - "flight_mode_2": "AUTONOMOUS", + "flight_mode_2": "DIRECT_THROTTLE_4DOF", "flight_mode_3": "DIRECT_THROTTLE_4DOF", "dsm_thr_ch": 1, diff --git a/src/settings.c b/src/settings.c index d1a4673..2310c3b 100644 --- a/src/settings.c +++ b/src/settings.c @@ -229,6 +229,10 @@ static int __parse_thrust_map(void) { settings.thrust_map = RX2206_4S; } + else if (strcmp(tmp_str, "AIR2213_3S") == 0) + { + settings.thrust_map = AIR2213_3S; + } else { fprintf(stderr, "ERROR: invalid thrust_map string\n"); diff --git a/src/thrust_map.c b/src/thrust_map.c index fbe45df..3f5bebc 100644 --- a/src/thrust_map.c +++ b/src/thrust_map.c @@ -97,6 +97,20 @@ static double rx2206_4s_map[][2] = { {0.905, 505.430124336786}, {1.0, 566.758535098236}}; +/* + * T-motor AIR 2213 920kv motor, 3S lipo, 8x4.5 MR prop + * T-motor AIR 20A 600Hz esc + */ +static const int air2213_3s_points = 22; +static double air2213_3s_map[][2] = {{0.000, 0.0}, {0.127, 0.022556517697878}, + {0.168, 0.146930900746933}, {0.209, 0.290547149026484}, {0.250, 0.453283925181439}, + {0.292, 0.636337098755366}, {0.333, 0.821683249995852}, {0.374, 1.00901761511471}, + {0.415, 1.20659090143905}, {0.456, 1.39841671964909}, {0.497, 1.63818127442106}, + {0.539, 1.93095707576772}, {0.580, 2.26657347053131}, {0.621, 2.66038767610247}, + {0.662, 3.04036747880942}, {0.703, 3.48544790945891}, {0.744, 3.89440663076139}, + {0.785, 4.3262274318871}, {0.827, 4.76347857182283}, {0.868, 5.1475601251139}, + {0.909, 5.67761582028645}, {1.000, 5.68764923230496}}; + // clang-format on int thrust_map_init(thrust_map_t map) @@ -123,6 +137,10 @@ int thrust_map_init(thrust_map_t map) points = rx2206_4s_points; data = rx2206_4s_map; break; + case AIR2213_3S: + points = air2213_3s_points; + data = air2213_3s_map; + break; default: fprintf(stderr, "ERROR: unknown thrust map\n"); return -1; From 7c743afe2ec2c9859987edb46eb9ea05ebc82c3b Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Tue, 20 Aug 2019 08:56:31 -0400 Subject: [PATCH 7/9] Changed alt_bmp to state estimate.Z outise of state estimator. --- src/feedback.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/feedback.c b/src/feedback.c index a57488e..67b00c4 100644 --- a/src/feedback.c +++ b/src/feedback.c @@ -223,7 +223,7 @@ int feedback_march(void) { if (last_en_Z_ctrl == 0) { - setpoint.Z = state_estimate.alt_bmp; // set altitude setpoint to current altitude + setpoint.Z = state_estimate.Z; // set altitude setpoint to current altitude rc_filter_reset(&D_Z); tmp = -setpoint.Z_throttle / (cos(state_estimate.roll) * cos(state_estimate.pitch)); rc_filter_prefill_outputs(&D_Z, tmp); @@ -231,7 +231,7 @@ int feedback_march(void) } D_Z.gain = D_Z_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; tmp = rc_filter_march( - &D_Z, -setpoint.Z + state_estimate.alt_bmp); // altitude is positive but +Z is down + &D_Z, -setpoint.Z + state_estimate.Z); // altitude is positive but +Z is down rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); u[VEC_Z] = tmp / cos(state_estimate.roll) * cos(state_estimate.pitch); mix_add_input(u[VEC_Z], VEC_Z, mot); From b5e1f8c3de1d60901c468501b61ff61f1e7c652f Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Tue, 20 Aug 2019 10:27:45 -0400 Subject: [PATCH 8/9] Added more logging, integrated xbee receive, verified xbee receive working properly --- include/input_manager.h | 1 + include/log_manager.h | 57 +++++- include/serial_com.h | 73 +++++++ include/settings.h | 5 + include/state_estimator.h | 10 +- include/xbee_receive.h | 63 ++++++ settings/hex_6dof_settings.json | 5 + settings/jellyfish_settings.json | 2 +- settings/pgaskell_settings.json | 5 + src/input_manager.c | 2 + src/log_manager.c | 179 +++++++++++----- src/main.c | 14 +- src/printf_manager.c | 25 ++- src/serial_com.c | 339 +++++++++++++++++++++++++++++++ src/settings.c | 5 + src/state_estimator.c | 2 + src/xbee_receive.c | 228 +++++++++++++++++++++ 17 files changed, 947 insertions(+), 68 deletions(-) create mode 100644 include/serial_com.h create mode 100644 include/xbee_receive.h create mode 100644 src/serial_com.c create mode 100644 src/xbee_receive.c diff --git a/include/input_manager.h b/include/input_manager.h index 9da03c9..5a7146a 100644 --- a/include/input_manager.h +++ b/include/input_manager.h @@ -41,6 +41,7 @@ typedef struct user_input_t int initialized; ///< set to 1 after input_manager_init(void) flight_mode_t flight_mode; ///< this is the user commanded flight_mode. int input_active; ///< nonzero indicates some user control is coming in + int kill_switch; ///< for printing arm_state_t requested_arm_mode; ///< set to ARMED after arming sequence is entered. // All sticks scaled from -1 to 1 diff --git a/include/log_manager.h b/include/log_manager.h index bd1834d..c8bc7f5 100644 --- a/include/log_manager.h +++ b/include/log_manager.h @@ -1,9 +1,11 @@ /** * * - * @brief Functions to start, stop, and interact with the log manager - * thread. + * @brief Functions to start, stop, and interact with the log manager + * thread. * + * @addtogroup LogManager + * @{ */ #ifndef LOG_MANAGER_H @@ -20,8 +22,9 @@ typedef struct log_entry_t { /** @name index, always printed */ ///@{ - uint64_t loop_index; // timing + uint64_t loop_index; uint64_t last_step_ns; + uint64_t imu_time_ns; ///< Time that ISR occurs ///@} /** @name sensors */ @@ -49,6 +52,29 @@ typedef struct log_entry_t double Zdot; ///@} + /*** @name xbee data */ + ///@{ + uint32_t xbee_time; + uint64_t xbee_time_received_ns; + float xbee_x; + float xbee_y; + float xbee_z; + float xbee_qw; + float xbee_qx; + float xbee_qy; + float xbee_qz; + ///@} + + /** @name throttles */ + ///@{ + double X_throttle; + double Y_throttle; + double Z_throttle; + double roll_throttle; + double pitch_throttle; + double yaw_throttle; + ///@} + /** @name setpoint */ ///@{ double sp_roll; @@ -84,32 +110,43 @@ typedef struct log_entry_t double mot_8; ///@} + /** @name dsm connection valid */ + ///@{ + int dsm_con; + ///@} + + /** @name flight mode */ + ///@{ + int flight_mode; + ///@} } log_entry_t; /** - * @brief creates a new csv log file and starts the background thread. + * @brief creates a new csv log file and starts the background thread. * - * @return 0 on success, -1 on failure + * @return 0 on success, -1 on failure */ int log_manager_init(void); /** - * @brief quickly add new data to local buffer + * @brief quickly add new data to local buffer * * This is called after feedback_march after signals have been sent to * the motors. * - * @return 0 on success, -1 on failure + * @return 0 on success, -1 on failure */ int log_manager_add_new(); /** - * @brief Finish writing remaining data to log and close thread. + * @brief Finish writing remaining data to log and close thread. * - * Used in log_manager.c + * Used in log_manager.c * - * @return 0 on sucess and clean exit, -1 on exit timeout/force close. + * @return 0 on sucess and clean exit, -1 on exit timeout/force close. */ int log_manager_cleanup(void); #endif // LOG_MANAGER_H + +/* @} end group LogManager */ diff --git a/include/serial_com.h b/include/serial_com.h new file mode 100644 index 0000000..2381e73 --- /dev/null +++ b/include/serial_com.h @@ -0,0 +1,73 @@ +/** + * + * + * @brief Simplified serial utilities + * + * @author eolson@mit.edu (2004) + * + * @addtogroup SerialCom + * @{ + */ + +#ifndef SERIAL_COM_H +#define SERIAL_COM_H + +// clang-format off + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @brief Creates a basic fd, setting baud to 9600, raw data i/o + * (no flow control, no fancy character handling. Configures + * it for blocking reads. 8 data bits, 1 stop bit, no parity. + * + * @return the fd or -1 on error + */ +int serial_open(const char *port, int baud, int blocking); + +/** @brief Set the baud rate, where the baudrate is just the integer value + * desired. + * + * @return non-zero on error. + */ +int serial_setbaud(int fd, int baudrate); + +/** + * @brief Enable cts/rts flow control. + * + * @return non-zero on error. + */ +int serial_enablectsrts(int fd); + +/** + * @brief Enable xon/xoff flow control. + * + * @returns non-zero on error. + */ +int serial_enablexon(int fd); + +/** + * @brief Set the port to 8 data bits, 2 stop bits, no parity. + * + * @return non-zero on error. + */ +int serial_set_N82(int fd); + +/** + * @brief Closes the serial interface + * + * @return non-zero on error + */ +int serial_close(int fd); +#ifdef __cplusplus +} +#endif + +// clang-format on + +#endif /*__SERIAL_COM__ */ + +/* @} end group SerialCom */ \ No newline at end of file diff --git a/include/settings.h b/include/settings.h index 5c61de6..bde065f 100644 --- a/include/settings.h +++ b/include/settings.h @@ -74,6 +74,7 @@ typedef struct settings_t int printf_sticks; int printf_setpoint; int printf_u; + int printf_xbee; int printf_motors; int printf_mode; ///@} @@ -86,6 +87,10 @@ typedef struct settings_t int log_setpoint; int log_control_u; int log_motor_signals; + int log_throttles; + int log_xbee; + int log_dsm; + int log_flight_mode; ///@} /** @name mavlink stuff */ diff --git a/include/state_estimator.h b/include/state_estimator.h index 8bd9cc6..4726289 100644 --- a/include/state_estimator.h +++ b/include/state_estimator.h @@ -31,6 +31,7 @@ typedef struct state_estimate_t { int initialized; + uint64_t imu_time_ns; ///< Time when ISR occurs (for logging purposes) /** @name IMU (accel gyro) * Normalized Quaternion is straight from the DMP but converted to NED @@ -98,10 +99,11 @@ typedef struct state_estimate_t ///@{ int mocap_running; ///< 1 if motion capture data is recent and valid uint64_t mocap_timestamp_ns; ///< timestamp of last received packet in nanoseconds since boot - double pos_mocap[3]; ///< position in mocap frame, converted to NED if necessary - double quat_mocap[4]; ///< UAV orientation according to mocap - double tb_mocap[3]; ///< Tait-Bryan angles according to mocap - int is_active; ///< TODO used by mavlink manager, purpose unclear... (pg) + uint64_t xbee_time_received_ns; ///< timestamp of xbee message received + double pos_mocap[3]; ///< position in mocap frame, converted to NED if necessary + double quat_mocap[4]; ///< UAV orientation according to mocap + double tb_mocap[3]; ///< Tait-Bryan angles according to mocap + int is_active; ///< TODO used by mavlink manager, purpose unclear... (pg) ///@} /** @name Global Position Estimate diff --git a/include/xbee_receive.h b/include/xbee_receive.h new file mode 100644 index 0000000..9d86b23 --- /dev/null +++ b/include/xbee_receive.h @@ -0,0 +1,63 @@ +/** + * + * + * @brief Functions for connecting to and recieving xbee messages + * + * @addtogroup XbeeReceive + * @{ + */ + +#ifndef XBEE_RECEIVE_H +#define XBEE_RECEIVE_H + +#include + +/** + * @brief Possition and orientation data sent/received from xbee + * + * This message definition must be identical to the sending message. This depends on the ground + * station and should be checked prior to using. There are many different xbee_packet_t's floating + * around + */ +typedef struct __attribute__((packed)) xbee_packet_t +{ + uint32_t time; ///< Unique id for the rigid body being described + float x; ///< x-position in the Optitrack frame + float y; ///< y-position in the Optitrack frame + float z; ///< z-position in the Optitrack frame + float qx; ///< qx of quaternion + float qy; ///< qy of quaternion + float qz; ///< qz of quaternion + float qw; ///< qw of quaternion + uint32_t trackingValid; ///< (bool) of whether or not tracking was valid (0 or 1) +} xbee_packet_t; + +#define NUM_FRAMING_BYTES 4 ///< 2 START bytes + 2 Fletcher-16 checksum bytes +#define OPTI_DATA_LENGTH sizeof(xbee_packet_t) ///< Actual Packet Being Sent +#define OPTI_PACKET_LENGTH OPTI_DATA_LENGTH + NUM_FRAMING_BYTES + +extern xbee_packet_t xbeeMsg; +extern int xbee_portID; + +/** + * @brief Xbee initialization function + * + * @return 0 on success, -1 on failure + */ +int XBEE_init(); + +/** + * @brief Read message recieved from XBee + * + * @return 0 on success, -1 on failure + */ +int XBEE_getData(); + +/** + * @brief Print current XBee message to stdout + */ +void XBEE_printData(); + +#endif /*__XBEE_RECEIVE__ */ + +/* @} end group XbeeReceive */ \ No newline at end of file diff --git a/settings/hex_6dof_settings.json b/settings/hex_6dof_settings.json index 6832d81..370da52 100644 --- a/settings/hex_6dof_settings.json +++ b/settings/hex_6dof_settings.json @@ -37,6 +37,7 @@ "printf_u": true, "printf_motors": true, "printf_mode": true, + "printf_xbee": false, "enable_logging": false, "log_sensors": true, @@ -44,6 +45,10 @@ "log_setpoint": true, "log_control_u": true, "log_motor_signals": true, + "log_throttles":true, + "log_xbee":true, + "log_dsm":true, + "log_flight_mode":true, "dest_ip": "192.168.8.1", "my_sys_id": 1, diff --git a/settings/jellyfish_settings.json b/settings/jellyfish_settings.json index 537688b..9d786ce 100644 --- a/settings/jellyfish_settings.json +++ b/settings/jellyfish_settings.json @@ -39,7 +39,7 @@ "printf_u": false, "printf_motors": false, "printf_mode": true, - "printf_xbee": false, + "printf_xbee": true, "printf_tracking": true, "enable_logging": true, diff --git a/settings/pgaskell_settings.json b/settings/pgaskell_settings.json index 2978888..c966136 100644 --- a/settings/pgaskell_settings.json +++ b/settings/pgaskell_settings.json @@ -38,6 +38,7 @@ "printf_u": true, "printf_motors": true, "printf_mode": true, + "printf_xbee":false, "enable_logging": true, "log_sensors": true, @@ -45,6 +46,10 @@ "log_setpoint": true, "log_control_u": true, "log_motor_signals": true, + "log_throttles":true, + "log_xbee":true, + "log_dsm":true, + "log_flight_mode":true, "dest_ip": "192.168.8.1", "my_sys_id": 1, diff --git a/src/input_manager.c b/src/input_manager.c index ac76fa1..49adb55 100644 --- a/src/input_manager.c +++ b/src/input_manager.c @@ -118,10 +118,12 @@ void new_dsm_data_callback() if (new_kill <= 0.1) { kill_switch = DISARMED; + user_input.kill_switch = 0; user_input.requested_arm_mode = DISARMED; } else { + user_input.kill_switch = 1; kill_switch = ARMED; } break; diff --git a/src/log_manager.c b/src/log_manager.c index a7848f1..dcebd9d 100644 --- a/src/log_manager.c +++ b/src/log_manager.c @@ -25,110 +25,154 @@ #include #include #include +#include #define MAX_LOG_FILES 500 #define BUF_LEN 50 -static uint64_t num_entries; // number of entries logged so far -static int buffer_pos; // position in current buffer -static int current_buf; // 0 or 1 to indicate which buffer is being filled -static int needs_writing; // flag set to 1 if a buffer is full -static FILE* fd; // file descriptor for the log file +static uint64_t num_entries; ///< number of entries logged so far +static int buffer_pos; ///< position in current buffer +static int current_buf; ///< 0 or 1 to indicate which buffer is being filled +static int needs_writing; ///< flag set to 1 if a buffer is full +static FILE* log_fd; ///< file descriptor for the log file // array of two buffers so one can fill while writing the other to file static log_entry_t buffer[2][BUF_LEN]; // background thread and running flag -static pthread_t pthread; +static pthread_t log_thread; static int logging_enabled; // set to 0 to exit the write_thread -static int __write_header(FILE* fd) +static int __write_header(FILE* log_fd) { // always print loop index - fprintf(fd, "loop_index,last_step_ns"); + fprintf(log_fd, "loop_index,last_step_ns,imu_time_ns"); if (settings.log_sensors) { - fprintf(fd, ",v_batt,alt_bmp_raw,gyro_roll,gyro_pitch,gyro_yaw,accel_X,accel_Y,accel_Z"); + fprintf( + log_fd, ",v_batt,alt_bmp_raw,gyro_roll,gyro_pitch,gyro_yaw,accel_X,accel_Y,accel_Z"); } if (settings.log_state) { - fprintf(fd, ",roll,pitch,yaw,X,Y,Z,Xdot,Ydot,Zdot"); + fprintf(log_fd, ",roll,pitch,yaw,X,Y,Z,Xdot,Ydot,Zdot"); + } + + if (settings.log_xbee) + { + fprintf(log_fd, + ",xbee_time,xbee_time_received_ns,xbee_x,xbee_y,xbee_z,xbee_qw,xbee_qx,xbee_qy,xbee_" + "qz"); + } + + if (settings.log_throttles) + { + fprintf(log_fd, ",X_thrt,Y_thrt,Z_thrt,roll_thrt,pitch_thrt,yaw_thrt"); } if (settings.log_setpoint) { - fprintf(fd, ",sp_roll,sp_pitch,sp_yaw,sp_X,sp_Y,sp_Z,sp_Xdot,sp_Ydot,sp_Zdot"); + fprintf(log_fd, ",sp_roll,sp_pitch,sp_yaw,sp_X,sp_Y,sp_Z,sp_Xdot,sp_Ydot,sp_Zdot"); } if (settings.log_control_u) { - fprintf(fd, ",u_roll,u_pitch,u_yaw,u_X,u_Y,u_Z"); + fprintf(log_fd, ",u_roll,u_pitch,u_yaw,u_X,u_Y,u_Z"); } if (settings.log_motor_signals && settings.num_rotors == 8) { - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6,mot_7,mot_8"); + fprintf(log_fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6,mot_7,mot_8"); } if (settings.log_motor_signals && settings.num_rotors == 6) { - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6"); + fprintf(log_fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6"); } if (settings.log_motor_signals && settings.num_rotors == 4) { - fprintf(fd, ",mot_1,mot_2,mot_3,mot_4"); + fprintf(log_fd, ",mot_1,mot_2,mot_3,mot_4"); + } + if (settings.log_dsm) + { + fprintf(log_fd, ",dsm_con"); + } + if (settings.log_flight_mode) + { + fprintf(log_fd, ",flight_mode"); } - fprintf(fd, "\n"); + fprintf(log_fd, "\n"); return 0; } -static int __write_log_entry(FILE* fd, log_entry_t e) +static int __write_log_entry(FILE* log_fd, log_entry_t l) { // always print loop index - fprintf(fd, "%" PRIu64 ",%" PRIu64, e.loop_index, e.last_step_ns); + fprintf( + log_fd, "%" PRIu64 ",%" PRIu64 ",%" PRIu64, l.loop_index, l.last_step_ns, l.imu_time_ns); if (settings.log_sensors) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.v_batt, e.alt_bmp_raw, - e.gyro_roll, e.gyro_pitch, e.gyro_yaw, e.accel_X, e.accel_Y, e.accel_Z); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.v_batt, l.alt_bmp_raw, + l.gyro_roll, l.gyro_pitch, l.gyro_yaw, l.accel_X, l.accel_Y, l.accel_Z); } if (settings.log_state) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.roll, e.pitch, e.yaw, e.X, - e.Y, e.Z, e.Xdot, e.Ydot, e.Zdot); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.roll, l.pitch, l.yaw, + l.X, l.Y, l.Z, l.Xdot, l.Ydot, l.Zdot); + } + + if (settings.log_xbee) + { + fprintf(log_fd, ",%" PRIu32 ",%" PRIu64, l.xbee_time, l.xbee_time_received_ns); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.xbee_x, l.xbee_y, l.xbee_z, + l.xbee_qw, l.xbee_qx, l.xbee_qy, l.xbee_qz); + } + + if (settings.log_throttles) + { + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.X_throttle, l.Y_throttle, l.Z_throttle, + l.roll_throttle, l.pitch_throttle, l.yaw_throttle); } if (settings.log_setpoint) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.sp_roll, e.sp_pitch, - e.sp_yaw, e.sp_X, e.sp_Y, e.sp_Z, e.sp_Xdot, e.sp_Ydot, e.sp_Zdot); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.sp_roll, l.sp_pitch, + l.sp_yaw, l.sp_X, l.sp_Y, l.sp_Z, l.sp_Xdot, l.sp_Ydot, l.sp_Zdot); } if (settings.log_control_u) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.u_roll, e.u_pitch, e.u_yaw, e.u_X, e.u_Y, - e.u_Z); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.u_roll, l.u_pitch, l.u_yaw, l.u_X, + l.u_Y, l.u_Z); } if (settings.log_motor_signals && settings.num_rotors == 8) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, - e.mot_5, e.mot_6, e.mot_7, e.mot_8); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.mot_1, l.mot_2, l.mot_3, + l.mot_4, l.mot_5, l.mot_6, l.mot_7, l.mot_8); } if (settings.log_motor_signals && settings.num_rotors == 6) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, e.mot_5, - e.mot_6); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", l.mot_1, l.mot_2, l.mot_3, l.mot_4, + l.mot_5, l.mot_6); } if (settings.log_motor_signals && settings.num_rotors == 4) { - fprintf(fd, ",%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4); + fprintf(log_fd, ",%.4F,%.4F,%.4F,%.4F", l.mot_1, l.mot_2, l.mot_3, l.mot_4); + } + if (settings.log_dsm) + { + fprintf(log_fd, ",%i", l.dsm_con); + } + if (settings.log_flight_mode) + { + fprintf(log_fd, ",%i", l.flight_mode); } - fprintf(fd, "\n"); + fprintf(log_fd, "\n"); return 0; } @@ -142,15 +186,19 @@ static void* __log_manager_func(__attribute__((unused)) void* ptr) { // buffer to be written is opposite of one currently being filled if (current_buf == 0) + { buf_to_write = 1; + } else + { buf_to_write = 0; + } // write the full buffer to disk; for (i = 0; i < BUF_LEN; i++) { - __write_log_entry(fd, buffer[buf_to_write][i]); + __write_log_entry(log_fd, buffer[buf_to_write][i]); } - fflush(fd); + fflush(log_fd); needs_writing = 0; } rc_usleep(1000000 / LOG_MANAGER_HZ); @@ -161,10 +209,10 @@ static void* __log_manager_func(__attribute__((unused)) void* ptr) // printf("writing out remaining log file\n"); for (i = 0; i < buffer_pos; i++) { - __write_log_entry(fd, buffer[current_buf][i]); + __write_log_entry(log_fd, buffer[current_buf][i]); } - fflush(fd); - fclose(fd); + fflush(log_fd); + fclose(log_fd); // zero out state logging_enabled = 0; @@ -184,8 +232,6 @@ int log_manager_init() // if the thread if running, stop before starting a new log file if (logging_enabled) { - // fprintf(stderr,"ERROR: in start_log_manager, log manager already running.\n"); - // return -1; log_manager_cleanup(); } @@ -196,6 +242,7 @@ int log_manager_init() } // search for existing log files to determine the next number in the series + // TODO: more expressive log name for (i = 1; i <= MAX_LOG_FILES + 1; i++) { memset(&path, 0, sizeof(path)); @@ -214,15 +261,15 @@ int log_manager_init() return -1; } // create and open new file for writing - fd = fopen(path, "w+"); - if (fd == 0) + log_fd = fopen(path, "w+"); + if (log_fd == 0) { printf("ERROR: can't open log file for writing\n"); return -1; } // write header - __write_header(fd); + __write_header(log_fd); // start thread logging_enabled = 1; @@ -232,7 +279,7 @@ int log_manager_init() needs_writing = 0; // start logging thread - if (rc_pthread_create(&pthread, __log_manager_func, NULL, SCHED_FIFO, LOG_MANAGER_PRI) < 0) + if (rc_pthread_create(&log_thread, __log_manager_func, NULL, SCHED_FIFO, LOG_MANAGER_PRI) < 0) { fprintf(stderr, "ERROR in start_log_manager, failed to start thread\n"); return -1; @@ -246,6 +293,7 @@ static log_entry_t __construct_new_entry() log_entry_t l; l.loop_index = fstate.loop_index; l.last_step_ns = fstate.last_step_ns; + l.imu_time_ns = state_estimate.imu_time_ns; l.v_batt = state_estimate.v_batt_lp; l.alt_bmp_raw = state_estimate.alt_bmp_raw; @@ -256,18 +304,35 @@ static log_entry_t __construct_new_entry() l.accel_Y = state_estimate.accel[1]; l.accel_Z = state_estimate.accel[2]; - l.roll = state_estimate.tb_imu[0]; - l.pitch = state_estimate.tb_imu[1]; - l.yaw = state_estimate.tb_imu[2]; - l.X = state_estimate.pos_global[0]; - l.Y = state_estimate.pos_global[1]; - l.Z = state_estimate.pos_global[2]; + l.roll = state_estimate.roll; + l.pitch = state_estimate.pitch; + l.yaw = state_estimate.continuous_yaw; + l.X = state_estimate.X; + l.Y = state_estimate.Y; + l.Z = state_estimate.Z; l.Xdot = state_estimate.vel_global[0]; l.Ydot = state_estimate.vel_global[1]; l.Zdot = state_estimate.vel_global[2]; + l.xbee_time = xbeeMsg.time; + l.xbee_time_received_ns = state_estimate.xbee_time_received_ns; + l.xbee_x = xbeeMsg.x; + l.xbee_y = xbeeMsg.y; + l.xbee_z = xbeeMsg.z; + l.xbee_qw = xbeeMsg.qw; + l.xbee_qx = xbeeMsg.qx; + l.xbee_qy = xbeeMsg.qy; + l.xbee_qz = xbeeMsg.qz; + + l.X_throttle = setpoint.X_throttle; + l.Y_throttle = setpoint.Y_throttle; + l.Z_throttle = setpoint.Z_throttle; + l.roll_throttle = setpoint.roll_throttle; + l.pitch_throttle = setpoint.pitch_throttle; + l.yaw_throttle = setpoint.yaw_throttle; + l.sp_roll = setpoint.roll; - l.sp_pitch = setpoint.pitch; + l.sp_pitch = setpoint.roll; l.sp_yaw = setpoint.yaw; l.sp_X = setpoint.X; l.sp_Y = setpoint.Y; @@ -292,6 +357,10 @@ static log_entry_t __construct_new_entry() l.mot_7 = fstate.m[6]; l.mot_8 = fstate.m[7]; + l.dsm_con = user_input.input_active; + + l.flight_mode = user_input.flight_mode; + return l; } @@ -318,9 +387,13 @@ int log_manager_add_new() needs_writing = 1; // flag the writer to dump to disk // swap buffers if (current_buf == 0) + { current_buf = 1; + } else + { current_buf = 0; + } } return 0; } @@ -333,10 +406,14 @@ int log_manager_cleanup() // disable logging so the thread can stop and start multiple times // thread also exits on rc_get_state()==EXITING logging_enabled = 0; - int ret = rc_pthread_timed_join(pthread, NULL, LOG_MANAGER_TOUT); + int ret = rc_pthread_timed_join(log_thread, NULL, LOG_MANAGER_TOUT); if (ret == 1) + { fprintf(stderr, "WARNING: log_manager_thread exit timeout\n"); + } else if (ret == -1) + { fprintf(stderr, "ERROR: failed to join log_manager thread\n"); + } return ret; } diff --git a/src/main.c b/src/main.c index a462e56..accd6e4 100644 --- a/src/main.c +++ b/src/main.c @@ -27,6 +27,7 @@ #include // contains extern settings variable #include #include +#include #define FAIL(str) \ fprintf(stderr, str); \ @@ -102,11 +103,15 @@ void on_pause_press() */ static void __imu_isr(void) { - // printf("imu interupt...\n"); + // Log time that isr occurs + state_estimate.imu_time_ns = rc_nanos_since_boot(); + setpoint_manager_update(); state_estimator_march(); feedback_march(); + if (settings.enable_logging) log_manager_add_new(); + state_estimator_jobs_after_feedback(); } @@ -279,6 +284,13 @@ int main(int argc, char* argv[]) FAIL("ERROR: failed to init state_estimator") } + // set up XBEE serial link + printf("initializing xbee serial link.\n"); + if (XBEE_init() < 0) + { + FAIL("ERROR: failed to init xbee serial link") + } + // set up feedback controller printf("initializing feedback controller\n"); if (feedback_init() < 0) diff --git a/src/printf_manager.c b/src/printf_manager.c index 4854c82..1effd43 100644 --- a/src/printf_manager.c +++ b/src/printf_manager.c @@ -18,6 +18,7 @@ #include #include #include +#include static pthread_t printf_manager_thread; static int initialized = 0; @@ -79,6 +80,10 @@ static int __print_header() { printf("%s U0X | U1Y | U2Z | U3r | U4p | U5y |", __next_colour()); } + if (settings.printf_xbee) + { + printf("%s x_xb | y_xb | z_xb | qx_xb | qy_xb | qz_xb | qw_xb |", __next_colour()); + } if (settings.printf_motors) { printf("%s", __next_colour()); @@ -129,9 +134,13 @@ static void* __printf_manager_func(__attribute__((unused)) void* ptr) if (settings.printf_arm) { if (fstate.arm_state == ARMED) + { printf("%s ARMED %s |", KRED, KNRM); + } else + { printf("%sDISARMED%s|", KGRN, KNRM); + } } __reset_colour(); if (settings.printf_altitude) @@ -147,10 +156,14 @@ static void* __printf_manager_func(__attribute__((unused)) void* ptr) } if (settings.printf_sticks) { - if (user_input.requested_arm_mode == ARMED) + if (user_input.kill_switch == 1) + { printf("%s ARMED ", KRED); + } else + { printf("%sDISARMED", KGRN); + } printf(KGRN); printf("%s|%+5.2f|%+5.2f|%+5.2f|%+5.2f|", __next_colour(), user_input.thr_stick, user_input.roll_stick, user_input.pitch_stick, user_input.yaw_stick); @@ -165,6 +178,12 @@ static void* __printf_manager_func(__attribute__((unused)) void* ptr) printf("%s%+5.2f|%+5.2f|%+5.2f|%+5.2f|%+5.2f|%+5.2f|", __next_colour(), fstate.u[0], fstate.u[1], fstate.u[2], fstate.u[3], fstate.u[4], fstate.u[5]); } + if (settings.printf_xbee) + { + printf("%s%+5.2f |%+5.2f |%+5.2f | %+5.2f | %+5.2f | %+5.2f | %+5.2f |", + __next_colour(), xbeeMsg.x, xbeeMsg.y, xbeeMsg.z, xbeeMsg.qx, xbeeMsg.qy, + xbeeMsg.qz, xbeeMsg.qw); + } if (settings.printf_motors) { printf("%s", __next_colour()); @@ -210,9 +229,13 @@ int printf_cleanup() // wait for the thread to exit ret = rc_pthread_timed_join(printf_manager_thread, NULL, PRINTF_MANAGER_TOUT); if (ret == 1) + { fprintf(stderr, "WARNING: printf_manager_thread exit timeout\n"); + } else if (ret == -1) + { fprintf(stderr, "ERROR: failed to join printf_manager thread\n"); + } } initialized = 0; return ret; diff --git a/src/serial_com.c b/src/serial_com.c new file mode 100644 index 0000000..4be0d0a --- /dev/null +++ b/src/serial_com.c @@ -0,0 +1,339 @@ +/* + * @file serial_com.c + * + * Simplified serial utilities. As opposed to my earlier + * serial class, we let you use the fd (or FILE*) directly. + * + * eolson@mit.edu, 2004 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef __linux__ +#include +#define SUPPORT_HISPEED 1 +#endif + +#include + +#include "serial_com.h" + +static int serial_translate_baud(int inrate); +static int serial_set_baud(int fd, int baud); + +/** Creates a basic fd, setting baud to 9600, raw data i/o (no flow + control, no fancy character handling. Configures it for blocking + reads. + + Returns the fd, -1 on error +**/ +int serial_open(const char *port, int baud, int blocking) +{ + struct termios opts; + + int flags = O_RDWR | O_NOCTTY; + if (!blocking) flags |= O_NONBLOCK; + + int fd = open(port, flags, 0); + if (fd == -1) return -1; + + if (tcgetattr(fd, &opts)) + { + perror("tcgetattr"); + return -1; + } + + cfsetispeed(&opts, serial_translate_baud(9600)); + cfsetospeed(&opts, serial_translate_baud(9600)); + + cfmakeraw(&opts); + + // set one stop bit + opts.c_cflag &= ~CSTOPB; + + if (tcsetattr(fd, TCSANOW, &opts)) + { + perror("tcsetattr"); + return -1; + } + + tcflush(fd, TCIOFLUSH); + + serial_set_baud(fd, baud); + return fd; +} + +// parity = 0: none, 1: odd, 2: even +int serial_set_mode(int fd, int databits, int parity, int stopbits) +{ + struct termios opts; + + if (tcgetattr(fd, &opts)) + { + perror("tcgetattr"); + return -1; + } + + opts.c_cflag &= (~CSIZE); + if (databits == 5) + opts.c_cflag |= CS5; + else if (databits == 6) + opts.c_cflag |= CS6; + else if (databits == 7) + opts.c_cflag |= CS7; + else if (databits == 8) + opts.c_cflag |= CS8; + + opts.c_cflag &= (~PARENB); + if (parity != 0) opts.c_cflag |= PARENB; + + opts.c_cflag &= (~PARODD); + if (parity == 1) opts.c_cflag |= PARODD; + + opts.c_cflag &= (~CSTOPB); + if (stopbits == 2) opts.c_cflag |= CSTOPB; + + if (tcsetattr(fd, TCSANOW, &opts)) + { + perror("tcsetattr"); + return -1; + } + + return 0; +} + +int serial_set_N82(int fd) +{ + struct termios opts; + + if (tcgetattr(fd, &opts)) + { + perror("tcgetattr"); + return -1; + } + + opts.c_cflag &= ~CSIZE; + opts.c_cflag |= CS8; + opts.c_cflag |= CSTOPB; + + if (tcsetattr(fd, TCSANOW, &opts)) + { + perror("tcsetattr"); + return -1; + } + + return 0; +} + +/** Enable cts/rts flow control. + Returns non-zero on error. +**/ +int serial_set_ctsrts(int fd, int enable) +{ + struct termios opts; + + if (tcgetattr(fd, &opts)) + { + perror("tcgetattr"); + return -1; + } + + if (enable) + opts.c_cflag |= CRTSCTS; + else + opts.c_cflag &= (~(CRTSCTS)); + + if (tcsetattr(fd, TCSANOW, &opts)) + { + perror("tcsetattr"); + return -1; + } + + return 0; +} + +/** Enable xon/xoff flow control. + Returns non-zero on error. +**/ +int serial_set_xon(int fd, int enable) +{ + struct termios opts; + + if (tcgetattr(fd, &opts)) + { + perror("tcgetattr"); + return -1; + } + + if (enable) + opts.c_iflag |= (IXON | IXOFF); + else + opts.c_iflag &= (~(IXON | IXOFF)); + + if (tcsetattr(fd, TCSANOW, &opts)) + { + perror("tcsetattr"); + return -1; + } + + return 0; +} + +/** Set the baud rate, where the baudrate is just the integer value + desired. + + Returns non-zero on error. +**/ +int serial_set_baud(int fd, int baudrate) +{ + struct termios tios; +#ifdef SUPPORT_HISPEED + struct serial_struct ser; +#endif + + int baudratecode = serial_translate_baud(baudrate); + + if (baudratecode > 0) + { + // standard baud rate. + tcgetattr(fd, &tios); + cfsetispeed(&tios, baudratecode); + cfsetospeed(&tios, baudratecode); + tcflush(fd, TCIFLUSH); + tcsetattr(fd, TCSANOW, &tios); + +#ifdef SUPPORT_HISPEED + ioctl(fd, TIOCGSERIAL, &ser); + + ser.flags = (ser.flags & (~ASYNC_SPD_MASK)); + ser.custom_divisor = 0; + + ioctl(fd, TIOCSSERIAL, &ser); +#endif + } + else + { + // non-standard baud rate. +#ifdef SUPPORT_HISPEED + // printf("Setting custom divisor\n"); + + if (tcgetattr(fd, &tios)) perror("tcgetattr"); + + cfsetispeed(&tios, B38400); + cfsetospeed(&tios, B38400); + tcflush(fd, TCIFLUSH); + + if (tcsetattr(fd, TCSANOW, &tios)) perror("tcsetattr"); + + if (ioctl(fd, TIOCGSERIAL, &ser)) perror("ioctl TIOCGSERIAL"); + + ser.flags = (ser.flags & (~ASYNC_SPD_MASK)) | ASYNC_SPD_CUST; + ser.custom_divisor = (ser.baud_base + baudrate / 2) / baudrate; + ser.reserved_char[0] = 0; // what the hell does this do? + + // printf("baud_base %i\ndivisor %i\n", ser.baud_base,ser.custom_divisor); + + if (ioctl(fd, TIOCSSERIAL, &ser)) perror("ioctl TIOCSSERIAL"); +#endif + } + + tcflush(fd, TCIFLUSH); + + return 0; +} + +static int serial_translate_baud(int inrate) +{ + switch (inrate) + { + case 0: + return B0; + case 300: + return B300; + case 1200: + return B1200; + case 2400: + return B2400; + case 4800: + return B4800; + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; +#ifdef SUPPORT_HISPEED + case 460800: + return B460800; +#endif + default: + return -1; // do custom divisor + } +} + +int serial_close(int fd) +{ + return close(fd); +} + +int serial_set_dtr(int fd, int v) +{ + int status; + + if (ioctl(fd, TIOCMGET, &status)) + { + perror("TIOCMGET"); + return -1; + } + + if (v) + status |= TIOCM_DTR; + else + status &= ~TIOCM_DTR; + + if (ioctl(fd, TIOCMSET, &status)) + { + perror("TIOCMSET"); + return -1; + } + + return 0; +} + +int serial_set_rts(int fd, int v) +{ + int status; + + if (ioctl(fd, TIOCMGET, &status)) + { + perror("TIOCMGET"); + return -1; + } + + if (v) + status |= TIOCM_RTS; + else + status &= ~TIOCM_RTS; + + if (ioctl(fd, TIOCMSET, &status)) + { + perror("TIOCMSET"); + return -1; + } + + return 0; +} diff --git a/src/settings.c b/src/settings.c index 2310c3b..e25e67d 100644 --- a/src/settings.c +++ b/src/settings.c @@ -674,6 +674,7 @@ int settings_load_from_file(char* path) PARSE_BOOL(printf_sticks) PARSE_BOOL(printf_setpoint) PARSE_BOOL(printf_u) + PARSE_BOOL(printf_xbee) PARSE_BOOL(printf_motors) PARSE_BOOL(printf_mode) @@ -684,6 +685,10 @@ int settings_load_from_file(char* path) PARSE_BOOL(log_setpoint) PARSE_BOOL(log_control_u) PARSE_BOOL(log_motor_signals) + PARSE_BOOL(log_throttles) + PARSE_BOOL(log_xbee) + PARSE_BOOL(log_dsm) + PARSE_BOOL(log_flight_mode) // MAVLINK PARSE_STRING(dest_ip) diff --git a/src/state_estimator.c b/src/state_estimator.c index d685dec..3c7986b 100644 --- a/src/state_estimator.c +++ b/src/state_estimator.c @@ -20,6 +20,7 @@ #include #include #include +#include #define TWO_PI (M_PI * 2.0) @@ -339,6 +340,7 @@ int state_estimator_march(void) __imu_march(); __mag_march(); __altitude_march(); + XBEE_getData(); __feedback_select(); __mocap_check_timeout(); return 0; diff --git a/src/xbee_receive.c b/src/xbee_receive.c new file mode 100644 index 0000000..a96a685 --- /dev/null +++ b/src/xbee_receive.c @@ -0,0 +1,228 @@ +/* + * @file xbee_receive.c + * + * Updated code (Feb 2019) + * + * Using data structure for packets with: + * Two start bytes: 0x81, 0xA1 + * [Not included: Message ID (one byte), Message payload size (one byte) since we only have one + * message type] Message data (xbee_packet_t length) Fletcher-16 checksum (two bytes) computed + * starting with Message payload size + * + * Note: This MBin protocol is commonly used on embedded serial devices subject to errors + */ + +#include +#include +#include //one of these two is for memcpy +#include +#include // read / write + +// Below for PRId64 +#include + +#include + +#include +#include +#include + +xbee_packet_t xbeeMsg; // Defined as extern in xbee_receive.h +int xbee_portID; // Defined as extern in xbee_receive.h + +// Information local to this file +void XBEE_readRingBuffer(); +#define XBEE_RING_BUFSIZE 256 +#define XBEE_RING_INC(a) \ + (a < (XBEE_RING_BUFSIZE - 1)) ? (a + 1) : 0 // Increment ring buffer index macro +int XBEE_ring_overflow = 0, XBEE_rdIndex = 0, XBEE_wrIndex = 0; +unsigned char XBEE_ringbuffer[XBEE_RING_BUFSIZE]; + +int XBEE_init() +{ + int baudRate = 57600; + xbee_portID = serial_open("/dev/ttyS5", baudRate, 0); // Nonblocking = 0, BLOCKING = 1 + if (xbee_portID == -1) + { + printf("Failed to open Serial Port\n"); + return -1; + } + return 0; +} + +// Read message received from XBee; use ring buffer to assure no data loss +int XBEE_getData() +{ + int k; + unsigned char buffer; + + // Populate ring buffer + for (k = 0; k < XBEE_RING_BUFSIZE; k++) + { + if (XBEE_ring_overflow) + { // Overflow condition on last read attempt + if (XBEE_rdIndex == XBEE_wrIndex) + { // Indices equal: overflow still in effect + return -1; // Do not read data; reading data would cause overflow + break; + } + else + { + XBEE_ring_overflow = 0; // Reset overflow flag + } + } + + if (read(xbee_portID, &buffer, 1) > 0) + { // Read from buffer: returns 0 or -1 if no data + XBEE_ringbuffer[XBEE_wrIndex] = buffer; + XBEE_wrIndex = XBEE_RING_INC(XBEE_wrIndex); + if (XBEE_wrIndex == XBEE_rdIndex) XBEE_ring_overflow = 1; + } + else + { + break; + } + } + + // Read and process data in ring buffer; print data + XBEE_readRingBuffer(); + + // Check if "trackingValid" + if (rc_nanos_since_boot() > state_estimate.xbee_time_received_ns + 1e8) + { + xbeeMsg.trackingValid = 0; + } + + return 0; +} + +// XBEE_readRingBuffer() +#define XBEE_startByte1 0x81 +#define XBEE_startByte2 0xA1 +void XBEE_readRingBuffer() +{ + static unsigned char msgState = 0, msglength = 0; + static unsigned char msgdata[OPTI_DATA_LENGTH], ck0, ck1; + + while (XBEE_ring_overflow || (XBEE_rdIndex != XBEE_wrIndex)) + { + // Don't get ahead of the receiving data + if (XBEE_ring_overflow) + { + XBEE_ring_overflow = 0; // Reset buffer overflow flag + } + // Case 0: Current character is first message header byte + if ((XBEE_ringbuffer[XBEE_rdIndex] == XBEE_startByte1) && !msgState) + { + msgState = 1; + msglength = 0; + } + + // Case 1: Current character is second message header byte + else if (msgState == 1) + { + if (XBEE_ringbuffer[XBEE_rdIndex] == XBEE_startByte2) + { + msgState = 2; + } + else + { + msgState = 0; // Bad message + } + ck0 = 0; // Initialize Fletcher checksum bytes + ck1 = 0; + msglength = 0; // Initialize number of message payload data bytes read thusfar + } + + // Case 2: Read data bytes into msgdata[] array + else if (msgState == 2) + { + msgdata[msglength++] = XBEE_ringbuffer[XBEE_rdIndex]; + ck0 += XBEE_ringbuffer[XBEE_rdIndex]; + ck1 += ck0; + if (msglength == OPTI_DATA_LENGTH) msgState = 3; // Done reading data + } + + // Case 3: Read & check the first checksum byte + // Throw away data if full checksum doesn't match + else if (msgState == 3) + { + if (ck0 != XBEE_ringbuffer[XBEE_rdIndex]) + { + msgState = 0; + } + else + { + msgState = 4; + } + } + + // Case 4: Read & check the second checksum byte + else if (msgState == 4) + { + msgState = 0; // Done reading message data + if (ck1 == XBEE_ringbuffer[XBEE_rdIndex]) + { + // Valid message -- copy and print + memcpy(&xbeeMsg, msgdata, OPTI_DATA_LENGTH); + state_estimate.xbee_time_received_ns = rc_nanos_since_boot(); + } + } + XBEE_rdIndex = XBEE_RING_INC(XBEE_rdIndex); + } + + return; +} + +/////////////////////////////////////// +void XBEE_printData() +{ + // Print to terminal + printf("\r"); + // Time + if (xbeeMsg.time < 1000000) + printf(" %u |", xbeeMsg.time); + else if (xbeeMsg.time < 10000000) + printf(" %u |", xbeeMsg.time); + else if (xbeeMsg.time < 100000000) + printf(" %u |", xbeeMsg.time); + else + printf("%u |", xbeeMsg.time); + + // XYZ + if (xbeeMsg.x < 0) + printf("%7.6f |", xbeeMsg.x); + else + printf(" %7.6f |", xbeeMsg.x); + if (xbeeMsg.y < 0) + printf("%7.6f |", xbeeMsg.y); + else + printf(" %7.6f |", xbeeMsg.y); + if (xbeeMsg.z < 0) + printf("%7.6f |", xbeeMsg.z); + else + printf(" %7.6f |", xbeeMsg.z); + + // Quaternion + if (xbeeMsg.qx < 0) + printf("%7.6f |", xbeeMsg.qx); + else + printf(" %7.6f |", xbeeMsg.qx); + if (xbeeMsg.qy < 0) + printf("%7.6f |", xbeeMsg.qy); + else + printf(" %7.6f |", xbeeMsg.qy); + if (xbeeMsg.qz < 0) + printf("%7.6f |", xbeeMsg.qz); + else + printf(" %7.6f |", xbeeMsg.qz); + if (xbeeMsg.qw < 0) + printf("%7.6f |", xbeeMsg.qw); + else + printf(" %7.6f |", xbeeMsg.qw); + + // Tracking Valid + printf(" %u |", xbeeMsg.trackingValid); + + fflush(stdout); +} From d8305d9de3b27ab5359eaac61fdeae19e006fbb0 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Tue, 20 Aug 2019 11:05:00 -0400 Subject: [PATCH 9/9] Added some documentation --- Makefile | 4 ++ README.md | 82 +++++++++++++++++++++++++++++++++----- docs/Doxyfile | 29 ++++++-------- docs/src/mainpage.dox | 73 ++++++++++++++++++++++++++++++--- include/feedback.h | 4 ++ include/flight_mode.h | 9 ++++- include/input_manager.h | 7 +++- include/mavlink_manager.h | 7 +++- include/mix.h | 23 ++++++----- include/printf_manager.h | 9 ++++- include/rc_pilot_defs.h | 9 ++++- include/setpoint_manager.h | 30 +++++++++----- include/settings.h | 9 ++++- include/state_estimator.h | 9 ++++- include/thread_defs.h | 12 +++++- include/thrust_map.h | 12 ++++-- src/mix.c | 2 +- src/setpoint_manager.c | 3 +- 18 files changed, 263 insertions(+), 70 deletions(-) diff --git a/Makefile b/Makefile index 31637de..8ba7da3 100644 --- a/Makefile +++ b/Makefile @@ -46,6 +46,9 @@ debug: $(MAKE) $(MAKEFILE) DEBUGFLAG="-g -D DEBUG" @echo "$(TARGET) Make Debug Complete" +docs: + @cd docs; doxygen Doxyfile + install: @$(INSTALLDIR) $(DESTDIR)$(prefix)/bin @$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/bin @@ -66,3 +69,4 @@ runonboot: @$(LINK) $(DESTDIR)$(prefix)/bin/$(TARGET) $(LINKDIR)/$(LINKNAME) @echo "$(TARGET) Set to Run on Boot" +.PHONY: all debug docs clean install uninstall runonboot \ No newline at end of file diff --git a/README.md b/README.md index 0505a39..a0cd94b 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,78 @@ -Multirotor Flight Controller for the Robotics Cape +# Documentation +[Documentation for rc_pilot](http://www-personal.umich.edu/~ghaggin/rc_pilot/) -Please don't use this and expect it to work the way you want it to!!!!! -I wrote this purely for my own use. NOT FOR PUBLIC USE!!!! -It comes with no warranty, no manual, and I won't answer questions about -how it works. If it breaks something, you get to keep both pieces. +# Setup +## Install dependencies on Beaglebone: -However, enough people have asked me for it so here it is. +```shell +$ sudo apt install libjson-c-dev libjson-c3 +``` +## Install librobotcontrol on Beaglebone: -Note: depends on libjson-c-dev & libjson-c3 -sudo apt install libjson-c-dev libjson-c3 +The repo for the library is located on the [Strawson Design](https://github.com/StrawsonDesign/librobotcontrol) GitHub page. There is a bug in the 1.0.4 release version (```rc_duplicate_filter``` fails for initialized filter) so [install librobotcontrol from source](http://strawsondesign.com/docs/librobotcontrol/installation.html). -also libroboticscape >v0.4.0 + +## Transfer rc_pilot source to Beaglebone: +In the scripts folder there are some convenient scripts for moving files to and from the vehicle. These scripts were build and tested on Ubuntu 19.04 but have not been tested on any other system. They should work on any system with bash and rsync. To transfer the files to the vehicle run: + +```shell +$ ./scripts/transfer_rcpilot [usb|wifi] +``` + +## Build on Beaglebone (from project directory): + +```shell +$ make +``` + +## Run RC_pilot: + +```shell +$ cd bin +$ sudo ./rc_pilot -s ../settings/quad_settings.json +``` + +# Miscellaneous +## Building docs: + +Make sure that doxygen is installed on your system and run + +``` shell +$ make docs +``` + +Open the docs with the web browser of your choice + +```shell +$ firefox docs/html/index.html +$ google-chrome docs/html/index.html +``` + +## (COMING SOON) Testing: +Test cases are written using the [Boost 1.66](https://www.boost.org/users/history/version_1_66_0.html) testing suite. Look at make target test for system install location. This suite is not written with portability in mind so it may take some tweaking to get it to work right now. Typically, installing boost with the option ```--prefix=/usr/local``` should put the libraries and headers in the correct location but don't count on it. + +Run the test suit with the make target: +```shell +$ make test +``` + +### Adding Test Modules + +To add a test modules, create a new file _test.cpp and format it as follows: + +```c++ +#include + +BOOST_AUTO_TEST_SUIT(Test_) + +BOOST_AUTO_TEST_CASE(test1){...} + +BOOST_AUTO_TEST_CASE(test2){...} + +... + +BOOST_AUTO_TEST_CASE_END() +``` + +The make target will compile each test module into a single executable and run all test cases simultaneously. \ No newline at end of file diff --git a/docs/Doxyfile b/docs/Doxyfile index b523495..ceefe7c 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -171,7 +171,7 @@ STRIP_FROM_PATH = # specify the list of include paths that are normally passed to the compiler # using the -I flag. -STRIP_FROM_INC_PATH = ../library/include/ +STRIP_FROM_INC_PATH = ../include # If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but # less readable) file names. This can be useful is your file systems doesn't @@ -226,7 +226,7 @@ SEPARATE_MEMBER_PAGES = NO # uses this value to replace tabs by spaces in code fragments. # Minimum value: 1, maximum value: 16, default value: 4. -TAB_SIZE = 8 +TAB_SIZE = 4 # This tag can be used to specify a number of aliases that act as commands in # the documentation. An alias has the form: @@ -531,7 +531,7 @@ CASE_SENSE_NAMES = NO # scope will be hidden. # The default value is: NO. -HIDE_SCOPE_NAMES = YES +HIDE_SCOPE_NAMES = NO # If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will # append additional text to a page's title, such as Class Reference. If set to @@ -570,7 +570,7 @@ INLINE_INFO = YES # name. If set to NO, the members will appear in declaration order. # The default value is: YES. -SORT_MEMBER_DOCS = NO +SORT_MEMBER_DOCS = YES # If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief # descriptions of file, namespace and class members alphabetically by member @@ -703,7 +703,7 @@ FILE_VERSION_FILTER = # DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE # tag is left empty. -LAYOUT_FILE = src/DoxygenLayout.xml +LAYOUT_FILE = # The CITE_BIB_FILES tag can be used to specify one or more bib files containing # the reference definitions. This must be a list of .bib files. The .bib @@ -790,8 +790,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = ../include \ - ../src/ +INPUT = ../include/ ./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. Doxygen uses @@ -907,9 +906,7 @@ EXCLUDE_SYMBOLS = # that contain example code fragments that are included (see the \include # command). -EXAMPLE_PATH = ../examples/src/ \ - ../library/include \ - ../rc_project_template +EXAMPLE_PATH = # If the value of the EXAMPLE_PATH tag contains directories, you can use the # EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and @@ -923,7 +920,7 @@ EXAMPLE_PATTERNS = * # irrespective of the value of the RECURSIVE tag. # The default value is: NO. -EXAMPLE_RECURSIVE = YES +EXAMPLE_RECURSIVE = NO # The IMAGE_PATH tag can be used to specify one or more files or directories # that contain images that are to be included in the documentation (see the @@ -998,7 +995,7 @@ USE_MDFILE_AS_MAINPAGE = # also VERBATIM_HEADERS is set to NO. # The default value is: NO. -SOURCE_BROWSER = YES +SOURCE_BROWSER = NO # Setting the INLINE_SOURCES tag to YES will include the body of functions, # classes and enums directly into the documentation. @@ -1011,7 +1008,7 @@ INLINE_SOURCES = NO # Fortran comments will always remain visible. # The default value is: YES. -STRIP_CODE_COMMENTS = NO +STRIP_CODE_COMMENTS = YES # If the REFERENCED_BY_RELATION tag is set to YES then for each documented # function all documented functions referencing it will be listed. @@ -1491,14 +1488,14 @@ GENERATE_TREEVIEW = YES # Minimum value: 0, maximum value: 20, default value: 4. # This tag requires that the tag GENERATE_HTML is set to YES. -ENUM_VALUES_PER_LINE = 1 +ENUM_VALUES_PER_LINE = 4 # If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used # to set the initial width (in pixels) of the frame in which the tree is shown. # Minimum value: 0, maximum value: 1500, default value: 250. # This tag requires that the tag GENERATE_HTML is set to YES. -TREEVIEW_WIDTH = 300 +TREEVIEW_WIDTH = 250 # If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to # external symbols imported via tag files in a separate window. @@ -2062,7 +2059,7 @@ ENABLE_PREPROCESSING = YES # The default value is: NO. # This tag requires that the tag ENABLE_PREPROCESSING is set to YES. -MACRO_EXPANSION = YES +MACRO_EXPANSION = NO # If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then # the macro expansion is limited to the macros specified with the PREDEFINED and diff --git a/docs/src/mainpage.dox b/docs/src/mainpage.dox index 0241bbc..36dd72b 100644 --- a/docs/src/mainpage.dox +++ b/docs/src/mainpage.dox @@ -1,16 +1,79 @@ /** - \mainpage +# Setup +## Install dependencies on Beaglebone: + +~~~ +$ sudo apt install libjson-c-dev libjson-c3 +~~~ + +## Install librobotcontrol on Beaglebone: + +The repo for the library is located on the Strawson Design GitHub page. There is a bug in the 1.0.4 release version (rc_duplicate_filter fails for initialized filter) so install librobotcontrol from source. + +## Transfer rc_pilot source to Beaglebone: +In the scripts folder there are some convenient scripts for moving files to and from the vehicle. These scripts were build and tested on Ubuntu 19.04 but have not been tested on any other system. They should work on any system with bash and rsync. To transfer the files to the vehicle run: + +~~~ +$ ./scripts/transfer_rcpilot [usb|wifi] +~~~ + +## Build on Beaglebone (from project directory): + +~~~ +$ make +~~~ + +## Run RC_pilot: + +~~~ +$ cd bin +$ sudo ./rc_pilot -s ../settings/quad_settings.json +~~~ + +# Miscellaneous +## Building docs: + +Make sure that doxygen is installed on your system and run + +~~~ +$ make docs +~~~ + +Open the docs with the web browser of your choice + +~~~ +$ firefox html/index.html +$ google-chrome html/index.html +~~~ + +## (COMING SOON) Testing: +Test cases are written using the Boost 1.66. unit test framework. Look at make target test for system install location. This suite is not written with portability in mind so you may need to tweak the build system in order to get it to compile on your machine. The boost libraries should be isntalled in /usr/local/lib and the headers should be installed in /usr/local/boost_1_66_0 (check the Makefile to confirm this). + +Run the test suit with the make target: +~~~ +$ make test +~~~ + +### Adding Test Modules + +To add a test modules, create a new file _test.cpp and format it as follows: + +~~~ +#include +BOOST_AUTO_TEST_SUIT(Test_) -This package contains the C library and example/testing programs for the Robot Control project. This project began as a hardware interface for the Robotics Cape and later the BeagleBone Blue and was originally called Robotics_Cape_Installer. It grew to include an extensive math library for discrete time feedback control, as well as a plethora of POSIX-compliant functions for timing, multithreading, program flow, and lots more. Everything is aimed at developing robotics software on embedded computers. +BOOST_AUTO_TEST_CASE(test1){...} -This package ships with official BeagleBone images and is focused on, but not excludive to, the BeagleBoard platform. The library and example programs are primarily written in C, however there also exists the RCPY python interface to this package available at . +BOOST_AUTO_TEST_CASE(test2){...} -The master branch is always the most current but not necessarily stable. See [releases](https://github.com/StrawsonDesign/Robotics_Cape_Installer/releases) page for older stable versions or install from BeagleBoard.org repositories. +... -To get started, visit the [user manual](manual.html) +BOOST_AUTO_TEST_CASE_END() +~~~ +The make target will compile each test module into a single executable and run all test cases simultaneously. */ diff --git a/include/feedback.h b/include/feedback.h index 4b3bb56..e53e980 100644 --- a/include/feedback.h +++ b/include/feedback.h @@ -10,6 +10,8 @@ * feedback_march(void) will monitor the setpoint which is constantly being * changed by setpoint_manager(void). * + * @addtogroup Feedback + * @{ */ #ifndef FEEDBACK_H @@ -86,3 +88,5 @@ int feedback_arm(void); int feedback_cleanup(void); #endif // FEEDBACK_H + +/**@} end group Feedback */ diff --git a/include/flight_mode.h b/include/flight_mode.h index 1388705..f011a34 100644 --- a/include/flight_mode.h +++ b/include/flight_mode.h @@ -1,5 +1,10 @@ /** * + * + * @brief Flight mode definitions + * + * @addtogroup FlightModes + * @{ */ #ifndef FLIGHT_MODE_H @@ -70,4 +75,6 @@ typedef enum flight_mode_t } flight_mode_t; -#endif \ No newline at end of file +#endif + +/**@}end group Flight Modes */ diff --git a/include/input_manager.h b/include/input_manager.h index 5a7146a..547e04b 100644 --- a/include/input_manager.h +++ b/include/input_manager.h @@ -1,9 +1,12 @@ /** * * - * Functions to start and stop the input manager thread which is the translation + * @brief Functions to start and stop the input manager thread which is the translation * beween control inputs from DSM to the user_input struct which is read by the * setpoint manager. TODO: Allow other inputs such as mavlink + * + * @addtogroup InputManager + * @{ */ #ifndef INPUT_MANAGER_H @@ -75,3 +78,5 @@ int input_manager_init(void); int input_manager_cleanup(void); #endif // INPUT_MANAGER_H + +/**@}end group InputMaganger */ diff --git a/include/mavlink_manager.h b/include/mavlink_manager.h index cd5b9b2..b7319a3 100644 --- a/include/mavlink_manager.h +++ b/include/mavlink_manager.h @@ -1,7 +1,10 @@ /** * * - * Functions to start and stop the mavlink manager + * @brief Functions to start and stop the mavlink manager + * + * @addtogroup MavlinkManager + * @{ */ #ifndef MAVLINK_MANAGER_H @@ -22,3 +25,5 @@ int mavlink_manager_init(void); int mavlink_manager_cleanup(void); #endif // MAVLINK_MANAGER_H + +/* @} end group Mavlink Manager */ diff --git a/include/mix.h b/include/mix.h index eae2fb4..93b4934 100644 --- a/include/mix.h +++ b/include/mix.h @@ -1,14 +1,17 @@ /** * * - * @brief Functions to mix orthogonal inputs to motor controls - * - * MultiRotors are controlled by mixing roll, pitch, yaw, and - * throttle control oututs, a linear combination of which forms the - * control output to each motor. The coefficients to this - * combination is stored in a mixing matrix based on rotor layout. - * Also included here are functions to parse configuration strings - * and do the actual mixing. + * @brief Functions to mix orthogonal inputs to motor controls + * + * MultiRotors are controlled by mixing roll, pitch, yaw, and + * throttle control oututs, a linear combination of which forms the + * control output to each motor. The coefficients to this + * combination is stored in a mixing matrix based on rotor layout. + * Also included here are functions to parse configuration strings + * and do the actual mixing. + * + * @addtogroup Mix + * @{ */ #ifndef MIXING_MATRIX_H @@ -106,4 +109,6 @@ int mix_check_saturation(int ch, double* mot, double* min, double* max); */ int mix_add_input(double u, int ch, double* mot); -#endif // MIXING_MATRIX_H \ No newline at end of file +#endif // MIXING_MATRIX_H + +/* @} end group Mix */ diff --git a/include/printf_manager.h b/include/printf_manager.h index 24c910c..7a73690 100644 --- a/include/printf_manager.h +++ b/include/printf_manager.h @@ -1,8 +1,11 @@ /** * * - * @brief Functions to start and stop the printf mnaager which is a - * separate thread printing data to the console for debugging. + * @brief Functions to start and stop the printf mnaager which is a + * separate thread printing data to the console for debugging. + * + * @addtogroup PrintfManager + * @{ */ #ifndef PRINTF_MANAGER_H @@ -36,3 +39,5 @@ int printf_cleanup(void); int print_flight_mode(flight_mode_t mode); #endif // PRINTF_MANAGER_H + +/* @} end group PrintfManager */ diff --git a/include/rc_pilot_defs.h b/include/rc_pilot_defs.h index 5d43dbe..f38c524 100644 --- a/include/rc_pilot_defs.h +++ b/include/rc_pilot_defs.h @@ -1,7 +1,10 @@ /** * * - * @brief constants and parameters + * @brief Constants and parameters + * + * @addtogroup RcPilotDefs + * @{ */ #ifndef RC_PILOT_DEFS_H @@ -89,4 +92,6 @@ typedef enum arm_state_t //#define DEBUG -#endif // RC_PILOT_DEFS_H \ No newline at end of file +#endif // RC_PILOT_DEFS_H + +/* @} end group RcPilotDefs */ diff --git a/include/setpoint_manager.h b/include/setpoint_manager.h index 2d38961..38834fa 100644 --- a/include/setpoint_manager.h +++ b/include/setpoint_manager.h @@ -1,18 +1,24 @@ /** * * - * @brief Setpoint manager runs at the same rate as the feedback controller - * and is the interface between the user inputs (input manager) and - * the feedback controller setpoint. currently it contains very - * simply logic and runs very quickly which is why it's okay to run - * in the feedback ISR right before the feedback controller. In the - * future this is where go-home and other higher level autonomy will - * live. + * @brief Guidance module for the vehicle + * + * Setpoint manager runs at the same rate as the feedback controller + * and is the interface between the user inputs (input manager) and + * the feedback controller setpoint. currently it contains very + * simply logic and runs very quickly which is why it's okay to run + * in the feedback ISR right before the feedback controller. In the + * future this is where go-home and other higher level autonomy will + * live. + * + * This serves to allow the feedback controller to be as simple and + * clean as possible by putting all high-level manipulation of the + * setpoints here. Then feedback-controller only needs to march the + * filters and zero them out when arming or enabling controllers + * + * @addtogroup SetpointManager + * @{ * - * This serves to allow the feedback controller to be as simple and - * clean as possible by putting all high-level manipulation of the - * setpoints here. Then feedback-controller only needs to march the - * filters and zero them out when arming or enabling controllers */ #ifndef SETPOINT_MANAGER_H @@ -100,3 +106,5 @@ int setpoint_manager_update(void); int setpoint_manager_cleanup(void); #endif // SETPOINT_MANAGER_H + +/* @} end group SetpointManager */ diff --git a/include/settings.h b/include/settings.h index bde065f..c647643 100644 --- a/include/settings.h +++ b/include/settings.h @@ -1,7 +1,10 @@ /** - * + * * - * @brief Functions to read the json settings file + * @brief Functions to read the json settings file + * + * @addtogroup Settings + * @{ */ #ifndef SETTINGS_H @@ -135,3 +138,5 @@ int settings_load_from_file(char* path); int settings_print(void); #endif // SETTINGS_H + +/* @} end group Settings */ diff --git a/include/state_estimator.h b/include/state_estimator.h index 4726289..edcde85 100644 --- a/include/state_estimator.h +++ b/include/state_estimator.h @@ -1,11 +1,14 @@ /** - * @file state_estimator.h + * * - * @brief Functions to start and stop the state estimator + * @brief Navigation module for the vehicle * * This runs at the same rate as the feedback controller. * state_estimator_march() is called immediately before feedback_march() in the * IMU interrupt service routine. + * + * @addtogroup StateEstimator + * @{ */ #ifndef STATE_ESTIMATOR_H @@ -173,3 +176,5 @@ int state_estimator_jobs_after_feedback(void); int state_estimator_cleanup(void); #endif // STATE_ESTIMATOR_H + +/* @} end group StateEstimator */ diff --git a/include/thread_defs.h b/include/thread_defs.h index 3b9e245..dbc82ea 100644 --- a/include/thread_defs.h +++ b/include/thread_defs.h @@ -1,4 +1,11 @@ - +/** + * + * + * @brief Thread speeds, prioritites, and close timeouts + * + * @addtogroup ThreadDefs + * @{ + */ #ifndef THREAD_DEFS_H #define THREAD_DEFS_H @@ -16,4 +23,5 @@ #define BUTTON_EXIT_CHECK_HZ 10 #define BUTTON_EXIT_TIME_S 2 -#endif \ No newline at end of file +#endif +/* @} end group ThreadDefs */ diff --git a/include/thrust_map.h b/include/thrust_map.h index 602aa3d..02b4b42 100644 --- a/include/thrust_map.h +++ b/include/thrust_map.h @@ -1,10 +1,12 @@ /** - * @headerfile thrust_map.h + * * - * @brief Functions to start and stop the printf manager which is a - * separate thread printing data to the console for debugging. + * @brief Functions to start and stop the printf manager which is a + * separate thread printing data to the console for debugging. + * + * @addtogroup ThrustMap + * @{ */ - #ifndef THRUST_MAP_H #define THRUST_MAP_H @@ -40,3 +42,5 @@ int thrust_map_init(thrust_map_t map); double map_motor_signal(double m); #endif // THRUST_MAP_H + +/* @} end group ThrustMap */ diff --git a/src/mix.c b/src/mix.c index f4f9710..57d6ee4 100644 --- a/src/mix.c +++ b/src/mix.c @@ -1,5 +1,5 @@ /** - * @file mixing_matrix.c + * @file mix.c */ #include // for DBL_MAX diff --git a/src/setpoint_manager.c b/src/setpoint_manager.c index 01f9785..dfcbfac 100644 --- a/src/setpoint_manager.c +++ b/src/setpoint_manager.c @@ -1,8 +1,7 @@ /** * @file setpoint_manager.c - * - * **/ + #include #include #include // for memset