diff --git a/conf/airframes/tudelft/delfly.xml b/conf/airframes/tudelft/delfly.xml deleted file mode 100644 index c04ff795a7..0000000000 --- a/conf/airframes/tudelft/delfly.xml +++ /dev/null @@ -1,202 +0,0 @@ - - - - - - DelFly Outdoor Auto2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - - - - - - - - - - -
- -
- - - - -
- -
- - -
- -
- - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - -
- -
- - -
- -
- - - - - - -
- -
- - - - - - - - -
- - diff --git a/conf/airframes/tudelft/delfly_lisas.xml b/conf/airframes/tudelft/delfly_lisas.xml new file mode 100644 index 0000000000..972fc85ebc --- /dev/null +++ b/conf/airframes/tudelft/delfly_lisas.xml @@ -0,0 +1,255 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + +
+ +
+ + + + + +
+ + +
+ + + + + +
+ +
+ + +
+ +
+ +
+ +
+ + + + + +
+ +
+ + + +
+ + +
+ + + + + + + + + +
+ + diff --git a/conf/modules/stabilization_rotorcraft.xml b/conf/modules/stabilization_rotorcraft.xml index 17a097312b..bae1c1b4fa 100644 --- a/conf/modules/stabilization_rotorcraft.xml +++ b/conf/modules/stabilization_rotorcraft.xml @@ -7,6 +7,13 @@ Also provide the direct mode 'stabilization_none' As a durty hack, it also provides navigation and guidance: this should be done in the airframe at some point +
+ + + + + +
diff --git a/conf/radios/delfly_Rx31_DEVO10.xml b/conf/radios/delfly_Rx31_DEVO10.xml new file mode 100644 index 0000000000..77ab530fe1 --- /dev/null +++ b/conf/radios/delfly_Rx31_DEVO10.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/conf/userconf/tudelft/mk_conf.xml b/conf/userconf/tudelft/mk_conf.xml new file mode 100644 index 0000000000..ff50916202 --- /dev/null +++ b/conf/userconf/tudelft/mk_conf.xml @@ -0,0 +1,13 @@ + + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 1787c4d67b..0328394565 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -319,7 +319,7 @@ void guidance_h_read_rc(bool in_flight) stabilization_attitude_read_rc(in_flight, FALSE, FALSE); break; case GUIDANCE_H_MODE_HOVER: - stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE ); + stabilization_attitude_read_rc_setpoint_eulers_f(&guidance_h.rc_sp, in_flight, FALSE, FALSE); #if GUIDANCE_H_USE_SPEED_REF read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight); /* enable x,y velocity setpoints */ @@ -374,6 +374,12 @@ void guidance_h_run(bool in_flight) transition_run(false); } stabilization_attitude_run(in_flight); +#if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW) + if (in_flight) { + stabilization_filter_commands(); + } +#endif + break; case GUIDANCE_H_MODE_HOVER: @@ -605,7 +611,7 @@ void guidance_h_from_nav(bool in_flight) /* set final attitude setpoint */ int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading); stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, - heading_sp_i); + heading_sp_i); #endif #endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c index 748d602cf2..56ac99cf03 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization.c @@ -25,11 +25,66 @@ #include "firmwares/rotorcraft/stabilization.h" +#if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW) +#include "filters/low_pass_filter.h" +#endif + int32_t stabilization_cmd[COMMANDS_NB]; +#if STABILIZATION_FILTER_CMD_ROLL_PITCH +#ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF +#define STABILIZATION_FILTER_CMD_ROLL_CUTOFF 20.0 +#endif + +#ifndef STABILIZATION_FILTER_CMD_PITCH_CUTOFF +#define STABILIZATION_FILTER_CMD_PITCH_CUTOFF 20.0 +#endif + +struct SecondOrderLowPass_int filter_roll; +struct SecondOrderLowPass_int filter_pitch; +#endif + +#if STABILIZATION_FILTER_CMD_YAW +#ifndef STABILIZATION_FILTER_CMD_YAW_CUTOFF +#define STABILIZATION_FILTER_CMD_YAW_CUTOFF 20.0 +#endif + +struct SecondOrderLowPass_int filter_yaw; +#endif + void stabilization_init(void) { for (uint8_t i = 0; i < COMMANDS_NB; i++) { stabilization_cmd[i] = 0; } + + // Initialize low pass filters +#if STABILIZATION_FILTER_CMD_ROLL_PITCH + init_second_order_low_pass_int(&filter_roll, STABILIZATION_FILTER_CMD_ROLL_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY, + 0.0); + init_second_order_low_pass_int(&filter_pitch, STABILIZATION_FILTER_CMD_PITCH_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY, + 0.0); +#endif + +#if STABILIZATION_FILTER_CMD_YAW + init_second_order_low_pass_int(&filter_yaw, STABILIZATION_FILTER_CMD_YAW_CUTOFF, 0.7071, 1.0 / PERIODIC_FREQUENCY, 0.0); +#endif + +} + +void stabilization_filter_commands(void) +{ + /* Filter the commands & bound the result */ +#if STABILIZATION_FILTER_CMD_ROLL_PITCH + stabilization_cmd[COMMAND_ROLL] = update_second_order_low_pass_int(&filter_roll, stabilization_cmd[COMMAND_ROLL]); + stabilization_cmd[COMMAND_PITCH] = update_second_order_low_pass_int(&filter_pitch, stabilization_cmd[COMMAND_PITCH]); + + BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); +#endif +#if STABILIZATION_FILTER_CMD_YAW + stabilization_cmd[COMMAND_YAW] = update_second_order_low_pass_int(&filter_yaw, stabilization_cmd[COMMAND_YAW]); + + BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h index 88a0a18250..063c8dfad2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization.h @@ -31,6 +31,7 @@ #include "generated/airframe.h" extern void stabilization_init(void); +extern void stabilization_filter_commands(void); /** Stabilization commands. * Contains the resulting stabilization commands,