Skip to content

Commit

Permalink
[ctrl_module] example
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Jan 15, 2015
1 parent 9e8a343 commit 95639bc
Show file tree
Hide file tree
Showing 3 changed files with 84 additions and 21 deletions.
10 changes: 6 additions & 4 deletions conf/modules/ctrl_module_demo.xml
View file
Expand Up @@ -5,22 +5,24 @@
<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>

<header>
<file name="ctrl_module_demo.h"/>
</header>


<makefile target="ap">
<makefile>
<file name="ctrl_module_demo.c"/>
</makefile>

Expand Down
68 changes: 66 additions & 2 deletions sw/airborne/modules/ctrl/ctrl_module_demo.c
View file
Expand Up @@ -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) {}


27 changes: 12 additions & 15 deletions sw/airborne/modules/ctrl/ctrl_module_demo.h
View file
Expand Up @@ -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;


// 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_ */

2 comments on commit 95639bc

@longshike
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This new control demo is suitable for fixed-wing or quadrotor? And Is there a help document about how to add new control method with own code?

@flixr
Copy link
Member

@flixr flixr commented on 95639bc Mar 7, 2015

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is only for rotorcraft firmware. Sorry there is no more step-by-step document...
But using the ctrl_demo module as shell and implementing your stuff in there shouldn't be too hard.
See also http://docs.paparazziuav.org/latest/guidance__module_8h.html
Otherwise ask on the mailing list...

Please sign in to comment.