From 95639bca382ac21554a9aced00a0913f102533e8 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 23:39:24 +0100 Subject: [PATCH] [ctrl_module] example --- conf/modules/ctrl_module_demo.xml | 10 +-- sw/airborne/modules/ctrl/ctrl_module_demo.c | 68 ++++++++++++++++++++- sw/airborne/modules/ctrl/ctrl_module_demo.h | 27 ++++---- 3 files changed, 84 insertions(+), 21 deletions(-) diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml index f5b94b2933e..9a322330058 100644 --- a/conf/modules/ctrl_module_demo.xml +++ b/conf/modules/ctrl_module_demo.xml @@ -5,13 +5,16 @@ Demo Control Module - Attitude controller with own code + Rate-controller as module sample - + + + + @@ -19,8 +22,7 @@ - - + diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c index 4c27067df5d..90183dea11e 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.c +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -26,16 +26,80 @@ */ #include "modules/ctrl/ctrl_module_demo.h" +#include "state.h" +#include "subsystems/radio_control.h" +#include "firmwares/rotorcraft/stabilization.h" +struct ctrl_module_demo_struct { + int rc_x; + int rc_y; + int rc_z; + int rc_t; -void my_ctrl_init(void) +} ctrl_module_demo; + +float ctrl_module_demo_pr_ff_gain = 0.2f; // Pitch/Roll +float ctrl_module_demo_pr_d_gain = 0.1f; +float ctrl_module_demo_y_ff_gain = 0.4f; // Yaw +float ctrl_module_demo_y_d_gain = 0.05f; + +void ctrl_module_init(void); +void ctrl_module_run(bool_t in_flight); + +void ctrl_module_init(void) +{ + ctrl_module_demo.rc_x = 0; + ctrl_module_demo.rc_y = 0; + ctrl_module_demo.rc_z = 0; + ctrl_module_demo.rc_t = 0; +} + +// Old-fashened rate control without reference model nor attitude +void ctrl_module_run(bool_t in_flight) { + if (!in_flight) { + // Reset integrators + stabilization_cmd[COMMAND_ROLL] = 0; + stabilization_cmd[COMMAND_PITCH] = 0; + 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_THRUST] = ctrl_module_demo.rc_t; + } +} + + +//////////////////////////////////////////////////////////////////// +// Call our controller +// Implement own Horizontal loops +void guidance_h_module_enter(void) +{ + ctrl_module_init(); } -void my_ctrl_run(void) +void guidance_h_module_read_rc(void) { + // -MAX_PPRZ to MAX_PPRZ + ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE]; + ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL]; + ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH]; + ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW]; +} +void guidance_h_module_run(bool_t in_flight) +{ + // Call full inner-/outerloop / horizontal-/vertical controller: + 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) {} diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h index 85c63e86908..f1500c374d1 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.h +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -30,30 +30,27 @@ #include -// Demo Controller Module -extern void my_ctrl_init(void); -extern void my_ctrl_run(void); // Settings -extern int ctrl_module_demo_gain; - - +extern float ctrl_module_demo_pr_ff_gain; // Pitch/Roll +extern float ctrl_module_demo_pr_d_gain; +extern float ctrl_module_demo_y_ff_gain; // Yaw +extern float ctrl_module_demo_y_d_gain; +// Demo with own guidance_h +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE -// Vertical loop re-uses Alt-hold +// and own guidance_v #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE -// Horizontal mode is a specific controller -#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE - // Implement own Horizontal loops -inline void guidance_h_module_enter(void) { my_ctrl_init();} -inline void guidance_h_module_read_rc(void) {} -inline void guidance_h_module_run(bool_t in_flight) {my_ctrl_run();} +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 -inline void guidance_v_module_enter(void){} -inline void guidance_v_module_run(bool_t in_flight){} +extern void guidance_v_module_enter(void); +extern void guidance_v_module_run(bool_t in_flight); #endif /* HOVER_STABILIZATION_H_ */