Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-09-23 20:31:49 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

[modules] example vertical_ctrl_module_demo

Browse Source
This commit is contained in:
Felix Ruess
2015-09-13 23:15:14 +02:00
parent 6e3021c830
commit b54e54ea2a
6 changed files with 208 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/vertical_ctrl_module_demo.xml
+34
View File
@@ -0,0 +1,34 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="vertical_ctrl_module_demo" dir="ctrl">
<doc>
<description>
Demo Vertical Control Module.
Only for rotorcraft firmware.
Example on how to reads the sonar and directly control the thrust.
</description>
<define name="VERTICAL_CTRL_MODULE_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
<section name="VERTICAL_CTRL_MODULE" prefix="VERTICAL_CTRL_MODULE_">
<define name="PGAIN" value="1.0" description="P gain on height error"/>
<define name="IGAIN" value="0.01" description="I gain on summed height error"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="VertCtrlModDemo">
<dl_setting var="v_ctrl.setpoint" min="0" step="0.01" max="10" module="ctrl/vertical_ctrl_module_demo" shortname="height sp"/>
<dl_setting var="v_ctrl.pgain" min="0" step="0.01" max="1" module="ctrl/vertical_ctrl_module_demo" shortname="pgain" param="VERTICAL_CTRL_MODULE_PGAIN"/>
<dl_setting var="v_ctrl.igain" min="0" step="0.01" max="1" module="ctrl/vertical_ctrl_module_demo" shortname="igain" param="VERTICAL_CTRL_MODULE_IGAIN"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="vertical_ctrl_module_demo.h"/>
</header>
<makefile>
<file name="vertical_ctrl_module_demo.c"/>
</makefile>
</module>
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+4
View File
@@ -187,6 +187,10 @@ void guidance_v_init(void)
gv_adapt_init();
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
guidance_v_module_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
register_periodic_telemetry(DefaultPeriodic, "TUNE_VERT", send_tune_vert);
sw/airborne/modules/ctrl/ctrl_module_demo.c
+5
View File
@@ -102,6 +102,11 @@ void guidance_h_module_run(bool_t in_flight)
ctrl_module_run(in_flight);
}
void guidance_v_module_init(void)
{
// initialization of your custom vertical controller goes here
}
// Implement own Vertical loops
void guidance_v_module_enter(void)
{
sw/airborne/modules/ctrl/ctrl_module_demo.h
+1
View File
@@ -50,6 +50,7 @@ extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
// Implement own Vertical loops
extern void guidance_v_module_init(void);
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);
sw/airborne/modules/ctrl/vertical_ctrl_module_demo.c
+110
View File
@@ -0,0 +1,110 @@
/*
* Copyright (C) 2015
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ctrl/vertical_control_module.h
* @brief example vertical controller
*
*/
#include "modules/ctrl/vertical_ctrl_module_demo.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization.h"
/* Default sonar/agl to use */
#ifndef VERTICAL_CTRL_MODULE_AGL_ID
#define VERTICAL_CTRL_MODULE_AGL_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(VERTICAL_CTRL_MODULE_AGL_ID)
#ifndef VERTICAL_CTRL_MODULE_PGAIN
#define VERTICAL_CTRL_MODULE_PGAIN 1.0
#endif
#ifndef VERTICAL_CTRL_MODULE_IGAIN
#define VERTICAL_CTRL_MODULE_IGAIN 0.01
#endif
static abi_event agl_ev; ///< The altitude ABI event
/// Callback function of the ground altitude
static void vertical_ctrl_agl_cb(uint8_t sender_id __attribute__((unused)), float distance);
struct VerticalCtrlDemo v_ctrl;
void vertical_ctrl_module_init(void);
void vertical_ctrl_module_run(bool_t in_flight);
void vertical_ctrl_module_init(void)
{
v_ctrl.agl = 0.0f;
v_ctrl.setpoint = 1.0f;
v_ctrl.pgain = VERTICAL_CTRL_MODULE_PGAIN;
v_ctrl.igain = VERTICAL_CTRL_MODULE_IGAIN;
v_ctrl.sum_err = 0.0f;
// Subscribe to the altitude above ground level ABI messages
AbiBindMsgAGL(VERTICAL_CTRL_MODULE_AGL_ID, &agl_ev, vertical_ctrl_agl_cb);
}
void vertical_ctrl_module_run(bool_t in_flight)
{
if (!in_flight) {
// Reset integrators
v_ctrl.sum_err = 0;
stabilization_cmd[COMMAND_THRUST] = 0;
} else {
int32_t nominal_throttle = 0.5 * MAX_PPRZ;
float err = v_ctrl.setpoint - v_ctrl.agl;
int32_t thrust = nominal_throttle + v_ctrl.pgain * err + v_ctrl.igain * v_ctrl.sum_err;
Bound(thrust, 0, MAX_PPRZ);
stabilization_cmd[COMMAND_THRUST] = thrust;
v_ctrl.sum_err += err;
}
}
static void vertical_ctrl_agl_cb(uint8_t sender_id, float distance)
{
v_ctrl.agl = distance;
}
////////////////////////////////////////////////////////////////////
// Call our controller
void guidance_v_module_init(void)
{
vertical_ctrl_module_init();
}
void guidance_v_module_enter(void)
{
// reset integrator
v_ctrl.sum_err = 0.0f;
}
void guidance_v_module_run(bool_t in_flight)
{
vertical_ctrl_module_run(in_flight);
}
sw/airborne/modules/ctrl/vertical_ctrl_module_demo.h
+54
View File
@@ -0,0 +1,54 @@
/*
* Copyright (C) 2015
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ctrl/vertical_ctrl_module_demo.c
* @brief example vertical
*
* Implements an example vertical controller in a module.
*/
#ifndef VERTICAL_CTRL_MODULE_DEMO_H_
#define VERTICAL_CTRL_MODULE_DEMO_H_
#include "std.h"
struct VerticalCtrlDemo {
float agl;
float setpoint;
float pgain;
float igain;
float sum_err;
};
extern struct VerticalCtrlDemo v_ctrl;
// for example use the standard horizontal (hover) mode
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_HOVER
// and own guidance_v
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Implement own Vertical loops
extern void guidance_v_module_init(void);
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);
#endif /* VERTICAL_CTRL_MODULE_DEMO_H_ */