-
Notifications
You must be signed in to change notification settings - Fork 1.1k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[modules] wing-rotation controller with servo (#3143)
* [modules] wing-rotation controller with servo
- Loading branch information
Showing
6 changed files
with
760 additions
and
1 deletion.
There are no files selected for viewing
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,27 @@ | ||
<!DOCTYPE module SYSTEM "module.dtd"> | ||
<module name="wing_rotation_controller_servo" dir="rot_wing_drone"> | ||
<doc> | ||
<description>Module to control wing rotation servo command based on prefered angle setpoint</description> | ||
</doc> | ||
<settings> | ||
<dl_settings NAME="wing_rotation"> | ||
<dl_settings NAME="wing_rot"> | ||
<dl_setting MAX="90" MIN="0" STEP="1" VAR="wing_rotation_controller.wing_angle_deg_sp" shortname="wing_sp_deg"/> | ||
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_first_order_dynamics" shortname="first_dyn"/> | ||
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_second_order_dynamics" shortname="second_dyn"/> | ||
</dl_settings> | ||
</dl_settings> | ||
</settings> | ||
<header> | ||
<file name="wing_rotation_controller_servo.h"/> | ||
</header> | ||
<init fun="wing_rotation_init()"/> | ||
<periodic fun="wing_rotation_periodic()" freq="1.0"/> | ||
<periodic fun="wing_rotation_event()"/> | ||
<makefile> | ||
<configure name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" default="ADC_5" case="lower|upper"/> | ||
<define name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" value="$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/> | ||
<define name="USE_$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/> | ||
<file name="wing_rotation_controller_servo.c"/> | ||
</makefile> | ||
</module> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
155 changes: 155 additions & 0 deletions
155
sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,155 @@ | ||
/* | ||
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl> | ||
* | ||
* 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/rot_wing_drone/wing_rotation_controller_v3b.c" | ||
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl> | ||
* Module to control wing rotation servo command based on prefered angle setpoint | ||
*/ | ||
|
||
#include "modules/rot_wing_drone/wing_rotation_controller_servo.h" | ||
#include "modules/radio_control/radio_control.h" | ||
#include "firmwares/rotorcraft/guidance/guidance_h.h" | ||
#include "generated/airframe.h" | ||
#include "modules/core/abi.h" | ||
#include "mcu_periph/adc.h" | ||
|
||
#if USE_NPS | ||
#include "modules/actuators/actuators.h" | ||
#endif | ||
|
||
|
||
#if !USE_NPS | ||
|
||
#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION | ||
#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION ADC_5 | ||
#endif | ||
|
||
#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES | ||
#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES 16 | ||
#endif | ||
|
||
#endif // !USE_NPS | ||
|
||
#ifndef WING_ROTATION_CONTROLLER_FIRST_DYN | ||
#define WING_ROTATION_CONTROLLER_FIRST_DYN 0.001 | ||
#endif | ||
|
||
#ifndef WING_ROTATION_CONTROLLER_SECOND_DYN | ||
#define WING_ROTATION_CONTROLLER_SECOND_DYN 0.003 | ||
#endif | ||
|
||
// Parameters | ||
struct wing_rotation_controller_t wing_rotation_controller = {0}; | ||
|
||
|
||
#if !USE_NPS | ||
static struct adc_buf buf_wing_rot_pos; | ||
#endif | ||
|
||
|
||
// Inline functions | ||
inline void wing_rotation_adc_to_deg(void); | ||
inline void wing_rotation_compute_pprz_cmd(void); | ||
|
||
// Initialization | ||
void wing_rotation_init(void) | ||
{ | ||
// ADC init | ||
#if !USE_NPS | ||
adc_buf_channel(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES); | ||
#endif | ||
|
||
// Init Data | ||
wing_rotation_controller.wing_angle_virtual_deg_sp = 45; | ||
wing_rotation_controller.wing_rotation_first_order_dynamics = WING_ROTATION_CONTROLLER_FIRST_DYN; | ||
wing_rotation_controller.wing_rotation_second_order_dynamics = WING_ROTATION_CONTROLLER_SECOND_DYN; | ||
} | ||
|
||
void wing_rotation_periodic(void) | ||
{ | ||
// After 5 loops, set current setpoint and enable wing_rotation | ||
// freq = 1.0 Hz | ||
if (!wing_rotation_controller.initialized) { | ||
wing_rotation_controller.init_loop_count += 1; | ||
if (wing_rotation_controller.init_loop_count > 4) { | ||
wing_rotation_controller.initialized = true; | ||
wing_rotation_controller.wing_angle_deg_sp = 45.; | ||
} | ||
} | ||
} | ||
|
||
void wing_rotation_event(void) | ||
{ | ||
// Update Wing position sensor | ||
wing_rotation_adc_to_deg(); | ||
|
||
// Run control if initialized | ||
if (wing_rotation_controller.initialized) { | ||
|
||
// Setpoint checks | ||
Bound(wing_rotation_controller.wing_angle_deg_sp, 0., 90.); | ||
|
||
// Control the wing rotation position. | ||
wing_rotation_compute_pprz_cmd(); | ||
} | ||
} | ||
|
||
void wing_rotation_adc_to_deg(void) | ||
{ | ||
#if !USE_NPS | ||
// Read ADC | ||
wing_rotation_controller.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample; | ||
wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294; | ||
|
||
#else // !USE_NPS | ||
// Copy setpoint as actual angle in simulation | ||
wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp; | ||
#endif | ||
|
||
// SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback | ||
struct act_feedback_t feedback; | ||
feedback.idx = SERVO_ROTATION_MECH; | ||
feedback.position = 0.5 * M_PI - RadOfDeg(wing_rotation_controller.wing_angle_deg); | ||
feedback.set.position = true; | ||
|
||
// Send ABI message | ||
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); | ||
|
||
} | ||
|
||
void wing_rotation_compute_pprz_cmd(void) | ||
{ | ||
// Smooth accerelation and rate limited setpoint | ||
float angle_error = wing_rotation_controller.wing_angle_deg_sp - wing_rotation_controller.wing_angle_virtual_deg_sp; | ||
float speed_sp = wing_rotation_controller.wing_rotation_first_order_dynamics * angle_error; | ||
float speed_error = speed_sp - wing_rotation_controller.wing_rotation_speed; | ||
wing_rotation_controller.wing_rotation_speed += wing_rotation_controller.wing_rotation_second_order_dynamics * speed_error; | ||
wing_rotation_controller.wing_angle_virtual_deg_sp += wing_rotation_controller.wing_rotation_speed; | ||
|
||
// Send to actuators | ||
int32_t servo_pprz_cmd; | ||
servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ); | ||
Bound(servo_pprz_cmd, 0, MAX_PPRZ); | ||
wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd; | ||
|
||
actuators_pprz[SERVO_ROTATION_MECH] = servo_pprz_cmd; | ||
} | ||
|
||
|
57 changes: 57 additions & 0 deletions
57
sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,57 @@ | ||
/* | ||
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl> | ||
* | ||
* 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/rot_wing_drone/wing_rotation_controller_servo.h" | ||
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl> | ||
* Module to control wing rotation servo command based on prefered angle setpoint | ||
*/ | ||
|
||
#ifndef WING_ROTATION_CONTROLLER_SERVO_H | ||
#define WING_ROTATION_CONTROLLER_SERVO_H | ||
|
||
#include "std.h" | ||
|
||
extern void wing_rotation_init(void); | ||
extern void wing_rotation_periodic(void); | ||
extern void wing_rotation_event(void); | ||
|
||
// Paramaters | ||
struct wing_rotation_controller_t { | ||
float wing_angle_deg; ///< Wing angle measurement in degrees | ||
float wing_angle_deg_sp; ///< Wing angle setpoint in degrees | ||
|
||
int32_t servo_pprz_cmd; ///< Servo command in pprz | ||
uint16_t adc_wing_rotation; ///< ADC value of wing | ||
|
||
float wing_rotation_speed; ///< Rate limiter state variable 1 | ||
float wing_angle_virtual_deg_sp; ///< Rate limiter state variable 2 | ||
float wing_rotation_first_order_dynamics; ///< Rate limiter for wing rotation | ||
float wing_rotation_second_order_dynamics; ///< Acceleration limiter for wing rotation | ||
|
||
bool initialized; ///< Wing rotation controller initialized | ||
uint8_t init_loop_count; ///< Wing rotation controller initialization loop count | ||
}; | ||
|
||
// Setters | ||
#define SetWingAngleDegSp(_wing_angle_deg_sp) (wing_rotation_controller.wing_angle_deg_sp = _wing_angle_deg_sp) | ||
|
||
extern struct wing_rotation_controller_t wing_rotation_controller; | ||
|
||
#endif // WING_ROTATION_CONTROLLER_SERVO_H |