From 0b245c04dbfe9da6748968e2ed850c461d73938a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 7 Mar 2015 13:53:27 +0100 Subject: [PATCH] [modules] minor ctrl_module_demo cosmetics --- conf/modules/ctrl_module_demo.xml | 19 +++++++------- sw/airborne/modules/ctrl/ctrl_module_demo.c | 29 ++++++++++++--------- sw/airborne/modules/ctrl/ctrl_module_demo.h | 6 ++--- 3 files changed, 30 insertions(+), 24 deletions(-) diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml index 9a32233005..5e7a6a8fa9 100644 --- a/conf/modules/ctrl_module_demo.xml +++ b/conf/modules/ctrl_module_demo.xml @@ -3,19 +3,20 @@ - Demo Control Module - - Rate-controller as module sample + Demo Control Module. + Only for rotorcraft firmware. + Simple rate controler as example on how to integrate write and call your own controller in a module. - - - - - - + + + + + + +
diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c index 0e3182467e..1590702235 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.c +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -53,7 +53,7 @@ void ctrl_module_init(void) ctrl_module_demo.rc_t = 0; } -// Old-fashened rate control without reference model nor attitude +// simple rate control without reference model nor attitude void ctrl_module_run(bool_t in_flight) { if (!in_flight) { @@ -63,18 +63,17 @@ void ctrl_module_run(bool_t in_flight) stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 0; } else { - stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->p * - ctrl_module_demo_pr_d_gain; - stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->q * - ctrl_module_demo_pr_d_gain; - stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - stateGetBodyRates_i()->r * - ctrl_module_demo_y_d_gain; + stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - + stateGetBodyRates_i()->p * ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - + stateGetBodyRates_i()->q * ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - + stateGetBodyRates_i()->r * ctrl_module_demo_y_d_gain; stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t; } } - //////////////////////////////////////////////////////////////////// // Call our controller // Implement own Horizontal loops @@ -82,6 +81,7 @@ void guidance_h_module_enter(void) { ctrl_module_init(); } + void guidance_h_module_read_rc(void) { // -MAX_PPRZ to MAX_PPRZ @@ -97,8 +97,13 @@ void guidance_h_module_run(bool_t in_flight) ctrl_module_run(in_flight); } -// Implement own Horizontal loops -inline void guidance_v_module_enter(void) {} -inline void guidance_v_module_run(UNUSED bool_t in_flight) {} - +// Implement own Vertical loops +void guidance_v_module_enter(void) +{ + // your code that should be executed when entering this vertical mode goes here +} +void guidance_v_module_run(UNUSED bool_t in_flight) +{ + // your vertical controller goes here +} diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h index 07a99eef7b..acf74f9024 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.h +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -22,6 +22,7 @@ * @file modules/ctrl/ctrl_module_demo.c * @brief example empty controller * + * Implements an example simple rate controller in a module. */ #ifndef CTRL_MODULE_DEMO_H_ @@ -47,9 +48,8 @@ extern void guidance_h_module_enter(void); extern void guidance_h_module_read_rc(void); extern void guidance_h_module_run(bool_t in_flight); -// Implement own Horizontal loops +// Implement own Vertical loops extern void guidance_v_module_enter(void); extern void guidance_v_module_run(bool_t in_flight); - -#endif /* HOVER_STABILIZATION_H_ */ +#endif /* CTRL_MODULE_DEMO_H_ */