Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

PFC

Browse Source
This commit is contained in:
Christophe De Wagter
2024-09-26 19:50:31 +02:00
committed by Freek van Tienen
parent 5369e4ab0a
commit 4d50e8d61c
2 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/checks/pfc_actuators.c
+7 -6
View File
@@ -28,12 +28,13 @@
#include "core/abi.h"
#include "modules/datalink/telemetry.h"
#include <stdio.h>
#include <math.h>
/**
* Maximum error for the angle of the actuators (rad)
*/
#ifndef PFC_ACTUATORS_MAX_ANGLE_ERROR
#define PFC_ACTUATORS_MAX_ANGLE_ERROR 0.1f
#define PFC_ACTUATORS_MAX_ANGLE_ERROR 0.05f
#endif
/**
@@ -174,11 +175,11 @@ void pfc_actuators_run(void) {
case PFC_ACTUATOR_STATE_LOW:
// Check if the feedback is correct
if(act->feedback_id != 255 &&
(pfc_actuators.last_feedback < (act->low_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->low_feedback + pfc_actuators.last_feedback_err))) {
(!isfinite(pfc_actuators.last_feedback) || (pfc_actuators.last_feedback < (act->low_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->low_feedback + pfc_actuators.last_feedback_err)))) {
pfc_actuators_error("Actuator %d not responding correctly LOW %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->low_feedback + pfc_actuators.last_feedback_err);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
} else if(act->feedback_id2 != 255 &&
(pfc_actuators.last_feedback2 < (act->low_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->low_feedback + pfc_actuators.last_feedback_err2))) {
(!isfinite(pfc_actuators.last_feedback2) || (pfc_actuators.last_feedback2 < (act->low_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->low_feedback + pfc_actuators.last_feedback_err2)))) {
pfc_actuators_error("Actuator %d not responding correctly LOW2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->low_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->low_feedback + pfc_actuators.last_feedback_err2);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
}
@@ -194,11 +195,11 @@ void pfc_actuators_run(void) {
case PFC_ACTUATOR_STATE_HIGH:
// Check if the feedback is correct
if(act->feedback_id != 255 &&
(pfc_actuators.last_feedback < (act->high_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->high_feedback + pfc_actuators.last_feedback_err))) {
(!isfinite(pfc_actuators.last_feedback) || (pfc_actuators.last_feedback < (act->high_feedback - pfc_actuators.last_feedback_err) || pfc_actuators.last_feedback > (act->high_feedback + pfc_actuators.last_feedback_err)))) {
pfc_actuators_error("Actuator %d not responding correctly HIGH %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err, pfc_actuators.last_feedback, act->high_feedback + pfc_actuators.last_feedback_err);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
} else if(act->feedback_id2 != 255 &&
(pfc_actuators.last_feedback2 < (act->high_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->high_feedback + pfc_actuators.last_feedback_err2))) {
} else if(act->feedback_id2 != 255 &&
(!isfinite(pfc_actuators.last_feedback2) || (pfc_actuators.last_feedback2 < (act->high_feedback - pfc_actuators.last_feedback_err2) || pfc_actuators.last_feedback2 > (act->high_feedback + pfc_actuators.last_feedback_err2)))) {
pfc_actuators_error("Actuator %d not responding correctly HIGH2 %.2f < %.2f < %.2f", pfc_actuators.act_idx, act->high_feedback - pfc_actuators.last_feedback_err2, pfc_actuators.last_feedback2, act->high_feedback + pfc_actuators.last_feedback_err2);
pfc_actuators.state = PFC_ACTUATORS_STATE_ERROR;
}
sw/airborne/modules/checks/preflight_checks.c
+1 -1
View File
@@ -132,7 +132,7 @@ bool preflight_check(void)
if (error_msg[i] == PREFLIGHT_CHECK_SEPERATOR || i == (PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len)) {
DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, i - last_sendi, &error_msg[last_sendi]);
#if FLIGHTRECORDER_SDLOG
pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, strlen(error_msg[last_sendi]), error_msg[last_sendi]);
pprz_msg_send_INFO_MSG(&pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, i - last_sendi, &error_msg[last_sendi]);
#endif
last_sendi = i + 1;
}