Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[stabilization] set commands from indi actuators array (#3606)

Browse Source 
Make it possible to set the command vector from the stabilization INDI controller, when STABILIZATION_INDI_COMMANDS array is defined. The benefit is a simpler configuration of the control_laws section of the airframe file when using INDI.
Be careful that with this, a "correct" failsafe_value is needed for the commands, since it is the value applied in KILL mode. Some special keywords (e.g. MOTOR_STOP) are added to help avoiding mistakes.

Backward compatible if STABILIZATION_INDI_COMMANDS is not defined.

- add example airframe
- add special names for command failsafe values:
  - MOTOR_STOP = MIN_PPRZ = -9600
  - MAX_PPRZ = 9600
  - MOTOR_IDLE = 0
This commit is contained in:
Gautier Hattenberger
2026-02-19 10:46:12 +01:00
committed by GitHub
parent d13141477d
commit 979c01cd7c
4 changed files with 50 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+11 -7
View File
@@ -101,17 +101,18 @@
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="FR" failsafe_value="MOTOR_STOP"/>
<axis name="BR" failsafe_value="MOTOR_STOP"/>
<axis name="BL" failsafe_value="MOTOR_STOP"/>
<axis name="FL" failsafe_value="MOTOR_STOP"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<set servo="FR" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
<set servo="BR" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
<set servo="BL" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
<set servo="FL" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
<set servo="FR" value="@FR"/>
<set servo="BR" value="@BR"/>
<set servo="BL" value="@BL"/>
<set servo="FL" value="@FL"/>
</command_laws>
<section name="IMU" prefix="IMU_">
@@ -196,6 +197,9 @@
<!--Priority for each axis (roll, pitch, yaw and thrust)-->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
<!-- INDI outputs to commands association -->
<define name="COMMANDS" value="{COMMAND_FR, COMMAND_BR, COMMAND_BL, COMMAND_FL}"/>
</section>
<section name="GUIDANCE_INDI" prefix="GUIDANCE_INDI_">
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+32
View File
@@ -220,6 +220,10 @@ static bool act_thrust_mat[3][INDI_NUM_ACT] = {
};
#endif
#ifdef STABILIZATION_INDI_COMMANDS
static uint8_t act_to_commands[INDI_NUM_ACT] = STABILIZATION_INDI_COMMANDS;
#endif
#ifdef STABILIZATION_INDI_ACT_DYN
#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
@@ -773,6 +777,10 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s
// commit actuator command
actuators_pprz[i] = (int16_t) indi_u[i];
#ifdef STABILIZATION_INDI_COMMANDS
// copy to actuators to specified commands
cmd[act_to_commands[i]] = actuators_pprz[i];
#endif
// update thrust command such that the current is correctly estimated
cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_thrust_mat[2][i];
@@ -915,6 +923,30 @@ void get_actuator_state(void)
#endif
}
#ifdef STABILIZATION_INDI_COMMANDS
/** Redefine if actuators to commands mapping is defined
*
* copy stabilization commands to general commands
* if corresponding actuator axis is producing thrust, stop motor is needed
*/
void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on)
{
for (int i = 0; i < INDI_NUM_ACT; i++) {
bool is_motor = act_thrust_mat[0][i] || act_thrust_mat[1][i] || act_thrust_mat[2][i];
if (is_motor && !motors_on) {
cmd_out[act_to_commands[i]] = MIN_PPRZ;
} else {
cmd_out[act_to_commands[i]] = cmd_in[act_to_commands[i]];
}
}
// for GCS display
if (!motors_on) {
cmd_in[COMMAND_THRUST] = 0;
}
cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
}
#endif
/**
* @param ddx_error error in output change
* @param i row of the matrix element
sw/ext/opencv_bebop
+1 -1
Submodule sw/ext/opencv_bebop updated: 472e91798b...b2264ae3ee
sw/tools/generators/gen_airframe.ml
+6 -1
View File
@@ -349,7 +349,12 @@ let parse_ap_only_commands = fun out ap_only ->
let parse_command = fun out command no ->
let command_name = "COMMAND_"^ExtXml.attrib command "name" in
let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
let failsafe_value = match (ExtXml.attrib command "failsafe_value") with
| "MIN_PPRZ" | "MOTOR_STOP" -> -9600
| "MAX_PPRZ" -> 9600
| "MOTOR_IDLE" -> 0
| x -> int_of_string x
in
let group = ExtXml.attrib_or_default command "group" "REAL" in
define_out out command_name (string_of_int no);
{ failsafe_value = failsafe_value; foo = 0 ; group = group }