mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[module] Controller for in the windtunnel to perform step inputs (#2510)
Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
This commit is contained in:
Freek van Tienen
GitHub
@@ -0,0 +1,50 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ctrl_windtunnel" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Windtunnel controller for performing step inputs on flaps and throttle.
|
||||
</description>
|
||||
|
||||
<section name="WINDTUNNEL" prefix="CTRL_WINDTUNNEL_">
|
||||
<define name="STEPTIME" value="8.0" description="Time [seconds]"/>
|
||||
<define name="THR_MIN" value="0" description="Minimum throttle in the sequence of steps [MAX_PPRZ]"/>
|
||||
<define name="THR_MAX" value="0" description="Maximum throttle in the sequence of steps"/>
|
||||
<define name="THR_STEP" value="1200" description="Throttle steps"/>
|
||||
<define name="FLAP_MIN" value="-7200" description="Minimum flap in the sequence of steps"/>
|
||||
<define name="FLAP_MAX" value="0" description="Maximum flap in the sequence of steps"/>
|
||||
<define name="FLAP_STEP" value="2400" description="Flaps steps"/>
|
||||
</section>
|
||||
|
||||
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="WindtunnelCtrl">
|
||||
<dl_setting var="ctrl_windtunnel_steptime" min="0.1" step="0.1" max="120" module="ctrl/ctrl_windtunnel" shortname="steptime" param="CTRL_WINDTUNNEL_STEPTIME"/>
|
||||
|
||||
<dl_setting var="ctrl_windtunnel_throttle.min" min="0" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="thr_min" param="CTRL_WINDTUNNEL_THR_MIN"/>
|
||||
<dl_setting var="ctrl_windtunnel_throttle.max" min="0" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="thr_max" param="CTRL_WINDTUNNEL_THR_MAX"/>
|
||||
<dl_setting var="ctrl_windtunnel_throttle.step" min="0" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="thr_step" param="CTRL_WINDTUNNEL_THR_STEP"/>
|
||||
|
||||
<dl_setting var="ctrl_windtunnel_flaps.min" min="-9600" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="flaps_min" param="CTRL_WINDTUNNEL_FLAP_MIN"/>
|
||||
<dl_setting var="ctrl_windtunnel_flaps.max" min="-9600" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="flaps_max" param="CTRL_WINDTUNNEL_FLAP_MAX"/>
|
||||
<dl_setting var="ctrl_windtunnel_flaps.step" min="0" step="100" max="9600" module="ctrl/ctrl_windtunnel" shortname="flaps_step" param="CTRL_WINDTUNNEL_FLAP_STEP"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
<header>
|
||||
<file name="ctrl_windtunnel.h"/>
|
||||
</header>
|
||||
|
||||
<makefile>
|
||||
<file name="ctrl_windtunnel.c"/>
|
||||
</makefile>
|
||||
<makefile target="ap">
|
||||
<define name="GUIDANCE_V_MODE_MODULE_SETTING" value="GUIDANCE_V_MODE_HOVER"/>
|
||||
<define name="DGUIDANCE_H_MODE_MODULE_SETTING" value="GUIDANCE_H_MODE_MODULE"/>
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,180 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
*
|
||||
* 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/ctrl_windtunnel.c
|
||||
* @brief Windtunnel automatic step controller
|
||||
*
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/ctrl_windtunnel.h"
|
||||
#include "state.h"
|
||||
#include "subsystems/radio_control.h"
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
#include "subsystems/electrical.h"
|
||||
|
||||
#ifndef WINDTUNNEL_TO_BODY_PHI
|
||||
#define WINDTUNNEL_TO_BODY_PHI 0
|
||||
#endif
|
||||
|
||||
#ifndef WINDTUNNEL_TO_BODY_THETA
|
||||
#define WINDTUNNEL_TO_BODY_THETA 0
|
||||
#endif
|
||||
|
||||
#ifndef WINDTUNNEL_TO_BODY_PSI
|
||||
#define WINDTUNNEL_TO_BODY_PSI 0
|
||||
#endif
|
||||
|
||||
struct ctrl_windtunnel_struct {
|
||||
int rc_throttle;
|
||||
int rc_roll;
|
||||
int rc_pitch;
|
||||
int rc_yaw;
|
||||
struct OrientationReps rotation;
|
||||
} ctrl_windtunnel;
|
||||
|
||||
float ctrl_windtunnel_steptime = CTRL_WINDTUNNEL_STEPTIME;
|
||||
struct min_max_ctrl_t ctrl_windtunnel_throttle = {.min = CTRL_WINDTUNNEL_THR_MIN, .max = CTRL_WINDTUNNEL_THR_MAX, .step = CTRL_WINDTUNNEL_THR_STEP};
|
||||
struct min_max_ctrl_t ctrl_windtunnel_flaps = {.min = CTRL_WINDTUNNEL_FLAP_MIN, .max = CTRL_WINDTUNNEL_FLAP_MAX, .step = CTRL_WINDTUNNEL_FLAP_STEP};
|
||||
static float last_time = 0;
|
||||
|
||||
void ctrl_module_init(void);
|
||||
void ctrl_module_run(bool in_flight);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
static void send_windtunnel_meas(struct transport_tx *trans, struct link_device *dev)
|
||||
{
|
||||
struct FloatQuat windtunnel_to_body;
|
||||
struct FloatEulers windtunnel_to_body_e;
|
||||
struct FloatQuat *rotation = orientationGetQuat_f(&ctrl_windtunnel.rotation);
|
||||
struct FloatQuat *ned_to_body = stateGetNedToBodyQuat_f();
|
||||
float_quat_comp_inv(&windtunnel_to_body, ned_to_body, rotation);
|
||||
float_eulers_of_quat(&windtunnel_to_body_e, &windtunnel_to_body);
|
||||
|
||||
float aoa = DegOfRad(windtunnel_to_body_e.theta);
|
||||
float power = electrical.vsupply * electrical.current;
|
||||
pprz_msg_send_WINDTUNNEL_MEAS(trans, dev, AC_ID, &aoa, &air_data.airspeed, &electrical.vsupply, &electrical.current,
|
||||
&power, COMMANDS_NB, stabilization_cmd);
|
||||
}
|
||||
#endif
|
||||
|
||||
void ctrl_module_init(void)
|
||||
{
|
||||
ctrl_windtunnel.rc_throttle = 0;
|
||||
ctrl_windtunnel.rc_roll = 0;
|
||||
ctrl_windtunnel.rc_pitch = 0;
|
||||
ctrl_windtunnel.rc_yaw = 0;
|
||||
|
||||
// Create a rotation to the windtunnel from the body
|
||||
struct FloatEulers windtunnel_to_body_eulers =
|
||||
{WINDTUNNEL_TO_BODY_PHI, WINDTUNNEL_TO_BODY_THETA, WINDTUNNEL_TO_BODY_PSI};
|
||||
orientationSetEulers_f(&ctrl_windtunnel.rotation, &windtunnel_to_body_eulers);
|
||||
|
||||
#if PERIODIC_TELEMETRY
|
||||
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WINDTUNNEL_MEAS, send_windtunnel_meas);
|
||||
#endif
|
||||
}
|
||||
|
||||
void ctrl_module_run(bool in_flight __attribute__((unused)))
|
||||
{
|
||||
bool done = false;
|
||||
// Increase step in steptime
|
||||
if (ctrl_windtunnel.rc_throttle > (MAX_PPRZ / 2) && (get_sys_time_float() - last_time) > ctrl_windtunnel_steptime) {
|
||||
// Increase throttle step if flaps at the end
|
||||
if ((ctrl_windtunnel_flaps.current + ctrl_windtunnel_flaps.step) > ctrl_windtunnel_flaps.max) {
|
||||
// Only increase step if throttle is not at the end
|
||||
if ((ctrl_windtunnel_throttle.current + ctrl_windtunnel_throttle.step) <= ctrl_windtunnel_throttle.max) {
|
||||
ctrl_windtunnel_throttle.current += ctrl_windtunnel_throttle.step;
|
||||
ctrl_windtunnel_flaps.current = ctrl_windtunnel_flaps.min;
|
||||
last_time = get_sys_time_float();
|
||||
} else {
|
||||
// Finished
|
||||
done = true;
|
||||
}
|
||||
} else {
|
||||
// By default increase flaps
|
||||
ctrl_windtunnel_flaps.current += ctrl_windtunnel_flaps.step;
|
||||
|
||||
// Double the amount of steptime during double transition
|
||||
if ((ctrl_windtunnel_flaps.current == ctrl_windtunnel_flaps.min)) {
|
||||
last_time = get_sys_time_float() + ctrl_windtunnel_steptime;
|
||||
} else {
|
||||
last_time = get_sys_time_float();
|
||||
}
|
||||
}
|
||||
} else if (ctrl_windtunnel.rc_throttle < (MAX_PPRZ / 2)) {
|
||||
// RESET
|
||||
ctrl_windtunnel_throttle.current = ctrl_windtunnel_throttle.min;
|
||||
ctrl_windtunnel_flaps.current = ctrl_windtunnel_flaps.min;
|
||||
last_time = get_sys_time_float();
|
||||
}
|
||||
|
||||
stabilization_cmd[COMMAND_ROLL] = 0;
|
||||
stabilization_cmd[COMMAND_PITCH] = 0;
|
||||
stabilization_cmd[COMMAND_YAW] = 0;
|
||||
stabilization_cmd[COMMAND_THRUST] = (done) ? 0 : ctrl_windtunnel_throttle.current;
|
||||
stabilization_cmd[COMMAND_FLAPS] = (done) ? 0 : ctrl_windtunnel_flaps.current;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Call our controller
|
||||
// Implement own Horizontal loops
|
||||
void guidance_h_module_init(void)
|
||||
{
|
||||
ctrl_module_init();
|
||||
}
|
||||
|
||||
void guidance_h_module_enter(void)
|
||||
{
|
||||
ctrl_module_init();
|
||||
}
|
||||
|
||||
void guidance_h_module_read_rc(void)
|
||||
{
|
||||
// -MAX_PPRZ to MAX_PPRZ
|
||||
ctrl_windtunnel.rc_throttle = radio_control.values[RADIO_THROTTLE];
|
||||
ctrl_windtunnel.rc_roll = radio_control.values[RADIO_ROLL];
|
||||
ctrl_windtunnel.rc_pitch = radio_control.values[RADIO_PITCH];
|
||||
ctrl_windtunnel.rc_yaw = radio_control.values[RADIO_YAW];
|
||||
}
|
||||
|
||||
void guidance_h_module_run(bool in_flight)
|
||||
{
|
||||
// Call full inner-/outerloop / horizontal-/vertical controller:
|
||||
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)
|
||||
{
|
||||
// your code that should be executed when entering this vertical mode goes here
|
||||
}
|
||||
|
||||
void guidance_v_module_run(UNUSED bool in_flight)
|
||||
{
|
||||
// your vertical controller goes here
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Freek van Tienen
|
||||
*
|
||||
* 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/ctrl_windtunnel.h
|
||||
* @brief Windtunnel controller
|
||||
*
|
||||
* Implements a controller for the windtunnel which does some step controls
|
||||
*/
|
||||
|
||||
#ifndef CTRL_MODULE_WINDTUNNEL_H_
|
||||
#define CTRL_MODULE_WINDTUNNEL_H_
|
||||
|
||||
#include <std.h>
|
||||
|
||||
struct min_max_ctrl_t {
|
||||
float min;
|
||||
float max;
|
||||
float step;
|
||||
float current;
|
||||
};
|
||||
|
||||
// Settings
|
||||
extern float ctrl_windtunnel_steptime;
|
||||
extern struct min_max_ctrl_t ctrl_windtunnel_throttle;
|
||||
extern struct min_max_ctrl_t ctrl_windtunnel_flaps;
|
||||
|
||||
// Implement own Horizontal loops
|
||||
extern void guidance_h_module_init(void);
|
||||
extern void guidance_h_module_enter(void);
|
||||
extern void guidance_h_module_read_rc(void);
|
||||
extern void guidance_h_module_run(bool 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 in_flight);
|
||||
|
||||
#endif /* CTRL_MODULE_WINDTUNNEL_H_ */
|
||||
Reference in New Issue
Block a user