Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ctrl_module] example

Browse Source
This commit is contained in:
dewagter
2015-01-15 23:39:24 +01:00
parent 9e8a343e9d
commit 95639bca38
3 changed files with 88 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ctrl_module_demo.xml
+6 -4
View File
@@ -5,13 +5,16 @@
<description>
Demo Control Module
Attitude controller with own code
Rate-controller as module sample
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="CtrlModDemo">
<dl_setting var="ctrl_module_demo_gain" min="0" step="1" max="1" module="ctrl/ctrl_module_demo" shortname="gain"/>
<dl_setting var="ctrl_module_demo_pr_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_ff"/>
<dl_setting var="ctrl_module_demo_pr_d_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_d"/>
<dl_setting var="ctrl_module_demo_y_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_ff"/>
<dl_setting var="ctrl_module_demo_y_d_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_d"/>
</dl_settings> </dl_settings>
</settings>
@@ -19,8 +22,7 @@
<file name="ctrl_module_demo.h"/>
</header>
<makefile target="ap">
<makefile>
<file name="ctrl_module_demo.c"/>
</makefile>
sw/airborne/modules/ctrl/ctrl_module_demo.c
+68 -4
View File
@@ -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;
}
void my_ctrl_run(void)
{
// 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 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) {}
sw/airborne/modules/ctrl/ctrl_module_demo.h
+14 -17
View File
@@ -30,30 +30,27 @@
#include <std.h>
// 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;
// Vertical loop re-uses Alt-hold
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Horizontal mode is a specific controller
// Demo with own guidance_h
#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();}
// and own guidance_v
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Implement own Horizontal loops
inline void guidance_v_module_enter(void){}
inline void guidance_v_module_run(bool_t in_flight){}
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
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);
#endif /* HOVER_STABILIZATION_H_ */