diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml index 659b8e794d..4485dee5da 100644 --- a/conf/airframes/tudelft/rot_wing_v3d.xml +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -125,6 +125,7 @@ + @@ -236,25 +237,26 @@ - - - - - - - - - - - - + + + + + + + + + + + + + - - - - - + + + + +
@@ -303,6 +305,9 @@ + + + diff --git a/conf/modules/actuators_bebop.xml b/conf/modules/actuators_bebop.xml index d29d96e480..c3b24e737f 100644 --- a/conf/modules/actuators_bebop.xml +++ b/conf/modules/actuators_bebop.xml @@ -23,7 +23,7 @@ - + diff --git a/conf/modules/actuators_disco.xml b/conf/modules/actuators_disco.xml index 675313ed85..672a538db9 100644 --- a/conf/modules/actuators_disco.xml +++ b/conf/modules/actuators_disco.xml @@ -21,7 +21,7 @@ - + diff --git a/conf/modules/pfc_actuators.xml b/conf/modules/pfc_actuators.xml new file mode 100644 index 0000000000..a8d9a3eb2d --- /dev/null +++ b/conf/modules/pfc_actuators.xml @@ -0,0 +1,36 @@ + + + + + + Preform a pre flight check of the actuators and validate by looking at the feedback. + + + + + + + + + + + + + + + @datalink,preflight_checks + +
+ +
+ + + + + + + + + + + diff --git a/conf/modules/preflight_checks.xml b/conf/modules/preflight_checks.xml index cd4937d763..7658ddd207 100644 --- a/conf/modules/preflight_checks.xml +++ b/conf/modules/preflight_checks.xml @@ -18,6 +18,9 @@ + + @datalink +
diff --git a/conf/modules/sensors_hitl.xml b/conf/modules/sensors_hitl.xml index 067571d804..31797ec776 100644 --- a/conf/modules/sensors_hitl.xml +++ b/conf/modules/sensors_hitl.xml @@ -33,7 +33,7 @@ - + diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index 051ba1beb7..edc1e53780 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -141,11 +141,7 @@ void actuators_bebop_commit(void) // Send ABI message struct act_feedback_t feedback[4]; for (int i=0;i<4;i++) { -#ifdef SERVOS_BEBOP_OFFSET - feedback[i].idx = SERVOS_BEBOP_OFFSET + i; -#else - feedback[i].idx = SERVOS_DEFAULT_OFFSET + i; -#endif + feedback[i].idx = get_servo_idx(i); feedback[i].rpm = actuators_bebop.rpm_obs[i]; feedback[i].set.rpm = true; } diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index a429c4de94..47db6d574a 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -40,6 +40,11 @@ #include + +#ifndef SERVO_MOTOR_IDX +#warning "Disco actuators require a " +#endif + /** * private observation structure */ @@ -170,11 +175,7 @@ void actuators_disco_commit(void) // Send ABI message struct act_feedback_t feedback = {0}; -#ifdef SERVOS_DISCO_OFFSET - feedback.idx = SERVOS_DISCO_OFFSET; -#else - feedback.idx = SERVOS_DEFAULT_OFFSET; -#endif + feedback.idx = SERVO_MOTOR_IDX; feedback.rpm = actuators_disco.rpm_obs; feedback.set.rpm = true; AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1); diff --git a/sw/airborne/modules/actuators/actuators_uavcan.c b/sw/airborne/modules/actuators/actuators_uavcan.c index 511b1c87f4..a55fad4805 100644 --- a/sw/airborne/modules/actuators/actuators_uavcan.c +++ b/sw/airborne/modules/actuators/actuators_uavcan.c @@ -49,13 +49,12 @@ struct actuators_uavcan_telem_t { float position; }; +/* The transmitted actuator values */ #ifdef SERVOS_UAVCAN1_NB int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB]; -static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0}; #endif #ifdef SERVOS_UAVCAN2_NB int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB]; -static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0}; #endif #ifdef SERVOS_UAVCAN1CMD_NB int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB]; @@ -64,6 +63,23 @@ int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB]; int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB]; #endif +/* Set the actual telemetry length (ID's from actuators can't collide with the command version) */ +#if SERVOS_UAVCAN1CMD_NB > SERVOS_UAVCAN1_NB +#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1CMD_NB +static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1CMD_NB] = {0}; +#elif defined(SERVOS_UAVCAN1_NB) +#define UAVCAN1_TELEM_NB SERVOS_UAVCAN1_NB +static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB] = {0}; +#endif + +#if SERVOS_UAVCAN2CMD_NB > SERVOS_UAVCAN2_NB +#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2CMD_NB +static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2CMD_NB] = {0}; +#elif defined(SERVOS_UAVCAN2_NB) +#define UAVCAN2_TELEM_NB SERVOS_UAVCAN2_NB +static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB] = {0}; +#endif + /* UNUSED value for CMD */ #define UAVCAN_CMD_UNUSED (MIN_PPRZ-1) @@ -112,8 +128,8 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) { // Find the next set telemetry uint8_t offset = 0; -#ifdef SERVOS_UAVCAN1_NB - for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN1_NB; i++) { +#ifdef UAVCAN1_TELEM_NB + for(uint8_t i = esc_idx - offset; i < UAVCAN1_TELEM_NB; i++) { if(uavcan1_telem[i].set) { old_idx = i + offset; esc_idx = i + offset + add_idx; @@ -122,10 +138,10 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) { esc_idx = i + offset + 1; } } - offset += SERVOS_UAVCAN1_NB; + offset += UAVCAN1_TELEM_NB; #endif -#ifdef SERVOS_UAVCAN2_NB - for(uint8_t i = esc_idx - offset; i < SERVOS_UAVCAN2_NB; i++) { +#ifdef UAVCAN2_TELEM_NB + for(uint8_t i = esc_idx - offset; i < UAVCAN2_TELEM_NB; i++) { if(uavcan2_telem[i].set) { old_idx = i + offset; esc_idx = i + offset + add_idx; @@ -134,7 +150,7 @@ static struct actuators_uavcan_telem_t *actuators_uavcan_next_telem(void) { esc_idx = i + offset + 1; } } - offset += SERVOS_UAVCAN2_NB; + offset += UAVCAN2_TELEM_NB; #endif // Going round or no telemetry found @@ -173,16 +189,16 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR struct actuators_uavcan_telem_t *telem = NULL; uint8_t max_id = 0; -#ifdef SERVOS_UAVCAN1_NB +#ifdef UAVCAN1_TELEM_NB if (iface == &uavcan1) { telem = uavcan1_telem; - max_id = SERVOS_UAVCAN1_NB; + max_id = UAVCAN1_TELEM_NB; } #endif -#ifdef SERVOS_UAVCAN2_NB +#ifdef UAVCAN2_TELEM_NB if (iface == &uavcan2) { telem = uavcan2_telem; - max_id = SERVOS_UAVCAN2_NB; + max_id = UAVCAN2_TELEM_NB; } #endif @@ -206,37 +222,53 @@ static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardR #if UAVCAN_ACTUATORS_USE_CURRENT // Update total current electrical.current = 0; -#ifdef SERVOS_UAVCAN1_NB - for (uint8_t i = 0; i < SERVOS_UAVCAN1_NB; ++i) { +#ifdef UAVCAN1_TELEM_NB + for (uint8_t i = 0; i < UAVCAN1_TELEM_NB; ++i) { electrical.current += uavcan1_telem[i].current; } #endif -#ifdef SERVOS_UAVCAN2_NB - for (uint8_t i = 0; i < SERVOS_UAVCAN2_NB; ++i) { +#ifdef UAVCAN2_TELEM_NB + for (uint8_t i = 0; i < UAVCAN2_TELEM_NB; ++i) { electrical.current += uavcan2_telem[i].current; } #endif #endif // Feedback ABI RPM messages -#ifdef SERVOS_UAVCAN1_NB +#ifdef UAVCAN1_TELEM_NB if (iface == &uavcan1) { - struct act_feedback_t feedback; - feedback.idx = SERVOS_UAVCAN1_OFFSET + esc_idx; + struct act_feedback_t feedback = {0}; feedback.rpm = telem[esc_idx].rpm; feedback.set.rpm = true; +#ifdef SERVOS_UAVCAN1_NB + feedback.idx = get_servo_idx_UAVCAN1(esc_idx); +#endif +#ifdef SERVOS_UAVCAN1CMD_NB + if(esc_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) { + feedback.idx = get_servo_idx_UAVCAN1CMD(esc_idx); + } +#endif + // Send ABI message AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); } #endif -#ifdef SERVOS_UAVCAN2_NB +#ifdef UAVCAN2_TELEM_NB if (iface == &uavcan2) { - struct act_feedback_t feedback; - feedback.idx = SERVOS_UAVCAN2_OFFSET + esc_idx; + struct act_feedback_t feedback = {0}; feedback.rpm = telem[esc_idx].rpm; feedback.set.rpm = true; +#ifdef SERVOS_UAVCAN2_NB + feedback.idx = get_servo_idx_UAVCAN2(esc_idx); +#endif +#ifdef SERVOS_UAVCAN2CMD_NB + if(esc_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[esc_idx] != UAVCAN_CMD_UNUSED) { + feedback.idx = get_servo_idx_UAVCAN2CMD(esc_idx); + } +#endif + // Send ABI message AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); } @@ -253,16 +285,16 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca struct actuators_uavcan_telem_t *telem = NULL; uint8_t max_id = 0; -#ifdef SERVOS_UAVCAN1_NB +#ifdef UAVCAN1_TELEM_NB if (iface == &uavcan1) { telem = uavcan1_telem; - max_id = SERVOS_UAVCAN1_NB; + max_id = UAVCAN1_TELEM_NB; } #endif -#ifdef SERVOS_UAVCAN2_NB +#ifdef UAVCAN2_TELEM_NB if (iface == &uavcan2) { telem = uavcan2_telem; - max_id = SERVOS_UAVCAN2_NB; + max_id = UAVCAN2_TELEM_NB; } #endif @@ -276,25 +308,41 @@ static void actuators_uavcan_actuator_status_cb(struct uavcan_iface_t *iface, Ca canardDecodeScalar(transfer, 8, 16, true, (void *)&tmp_float); telem[actuator_idx].position = canardConvertFloat16ToNativeFloat(tmp_float); -#ifdef SERVOS_UAVCAN1_NB +#ifdef UAVCAN1_TELEM_NB if (iface == &uavcan1) { - struct act_feedback_t feedback; - feedback.idx = SERVOS_UAVCAN1_OFFSET + actuator_idx; + struct act_feedback_t feedback = {0}; feedback.position = telem[actuator_idx].position; feedback.set.position = true; +#ifdef SERVOS_UAVCAN1_NB + feedback.idx = get_servo_idx_UAVCAN1(actuator_idx); +#endif +#ifdef SERVOS_UAVCAN1CMD_NB + if(actuator_idx < SERVOS_UAVCAN1CMD_NB && actuators_uavcan1cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) { + feedback.idx = get_servo_idx_UAVCAN1CMD(actuator_idx); + } +#endif + // Send ABI message AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); } #endif -#ifdef SERVOS_UAVCAN2_NB +#ifdef UAVCAN2_TELEM_NB if (iface == &uavcan2) { - struct act_feedback_t feedback; - feedback.idx = SERVOS_UAVCAN2_OFFSET + actuator_idx; + struct act_feedback_t feedback = {0}; feedback.position = telem[actuator_idx].position; feedback.set.position = true; +#ifdef SERVOS_UAVCAN2_NB + feedback.idx = get_servo_idx_UAVCAN2(actuator_idx); +#endif +#ifdef SERVOS_UAVCAN2CMD_NB + if(actuator_idx < SERVOS_UAVCAN2CMD_NB && actuators_uavcan2cmd_values[actuator_idx] != UAVCAN_CMD_UNUSED) { + feedback.idx = get_servo_idx_UAVCAN2CMD(actuator_idx); + } +#endif + // Send ABI message AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); } @@ -311,16 +359,16 @@ static void actuators_uavcan_device_temperature_cb(struct uavcan_iface_t *iface, struct actuators_uavcan_telem_t *telem = NULL; uint8_t max_id = 0; -#ifdef SERVOS_UAVCAN1_NB +#ifdef UAVCAN1_TELEM_NB if (iface == &uavcan1) { telem = uavcan1_telem; - max_id = SERVOS_UAVCAN1_NB; + max_id = UAVCAN1_TELEM_NB; } #endif -#ifdef SERVOS_UAVCAN2_NB +#ifdef UAVCAN2_TELEM_NB if (iface == &uavcan2) { telem = uavcan2_telem; - max_id = SERVOS_UAVCAN2_NB; + max_id = UAVCAN2_TELEM_NB; } #endif diff --git a/sw/airborne/modules/checks/pfc_actuators.c b/sw/airborne/modules/checks/pfc_actuators.c new file mode 100644 index 0000000000..333e7f4465 --- /dev/null +++ b/sw/airborne/modules/checks/pfc_actuators.c @@ -0,0 +1,312 @@ +/* + * Copyright (C) 2023 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 + * . + */ + +/** + * @file "modules/checks/pfc_actuators.c" + * @author Freek van Tienen + * Checks the actuators with feedback before takeoff + */ + +#include "preflight_checks.h" +#include "core/abi.h" +#include "modules/datalink/telemetry.h" +#include + +/** + * Maximum error for the angle of the actuators (rad) +*/ +#ifndef PFC_ACTUATORS_MAX_ANGLE_ERROR +#define PFC_ACTUATORS_MAX_ANGLE_ERROR 0.1f +#endif + +/** + * Maximum error for the RPM of the actuators +*/ +#ifndef PFC_ACTUATORS_MAX_RPM_ERROR +#define PFC_ACTUATORS_MAX_RPM_ERROR 250.0f +#endif + +/** + * Enable debugging to set the expected feedback values +*/ +#ifndef PFC_ACTUATORS_DEBUG +#define PFC_ACTUATORS_DEBUG false +#endif + +/** + * @brief The status of the preflight checks +*/ +enum pfc_actuators_state_t { + PFC_ACTUATORS_STATE_INIT, + PFC_ACTUATORS_STATE_RUNNING, + PFC_ACTUATORS_STATE_SUCCESS, + PFC_ACTUATORS_STATE_ERROR +}; + +/** + * @brief The state of the actuator during the test +*/ +enum pfc_actuator_state_t { + PFC_ACTUATOR_STATE_WAIT, + PFC_ACTUATOR_STATE_LOW, + PFC_ACTUATOR_STATE_HIGH, +}; + +/** + * @brief The configuration struct of the actuator +*/ +struct pfc_actuator_t { + uint8_t feedback_id; ///< The feedback id of the actuator (255 for none) + uint8_t feedback_id2; ///< The secondary feedback id of the actuator (255 for none) + + int16_t low; ///< The low value to set the actuator to + int16_t high; ///< The high value to set the actuator to + float low_feedback; ///< The expected feedback value when the actuator is low + float high_feedback; ///< The expected feedback value when the actuator is high + + float timeout; ///< The timeout for the actuator to move +}; + +struct pfc_actuators_t { + enum pfc_actuators_state_t state; ///< The state of the preflight checks + + uint8_t act_idx; ///< The current actuator index + uint8_t act_nb; ///< The number of actuators + enum pfc_actuator_state_t act_state; ///< The state of the actuator (during the test) + float act_start_time; ///< The start time of the actuator (during the test) + + float last_feedback; ///< The last measured feedback value of the actuator + float last_feedback_err; ///< The last expected feedback error of the actuator (based on RPM/angle) + float last_feedback2; ///< The last measured secondary feedback value of the actuator + float last_feedback_err2; ///< The last expected secondary feedback error of the actuator (based on RPM/angle) +}; + +// Local variables and functions +static struct pfc_actuator_t pfc_acts[] = PFC_ACTUATORS; +static struct pfc_actuators_t pfc_actuators; +static struct preflight_check_t actuators_pfc; +static abi_event act_feedback_ev; +static void pfc_actuators_cb(struct preflight_result_t *result); +static void pfc_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act); + +/** + * @brief Send an error message to the ground station + * @param fmt The format of the message + * @param ... The arguments for the format + */ +static void pfc_actuators_error(const char *fmt, ...) { + char msg[200]; + + // Add the error + va_list args; + va_start(args, fmt); + int rc = vsnprintf(msg, 200, fmt, args); + va_end(args); + + if(rc > 0) { + DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, msg); + } +} + +/** + * @brief Send a debug message to the ground station +*/ +#if PFC_ACTUATORS_DEBUG +#define pfc_actuators_debug pfc_actuators_error +#else +#define pfc_actuators_debug(...) +#endif + +/** + * @brief Register the preflight checks for the actuators + */ +void pfc_actuators_init(void) { + pfc_actuators.state = PFC_ACTUATORS_STATE_INIT; + pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT; + pfc_actuators.act_idx = 0; + pfc_actuators.act_nb = sizeof(pfc_acts) / sizeof(struct pfc_actuator_t); + pfc_actuators.last_feedback = NAN; + pfc_actuators.last_feedback2 = NAN; + + preflight_check_register(&actuators_pfc, pfc_actuators_cb); + AbiBindMsgACT_FEEDBACK(ABI_BROADCAST, &act_feedback_ev, pfc_act_feedback_cb); +} + +/** + * @brief Move the actuators, should be put in the command laws + */ +void pfc_actuators_run(void) { + // Only actuate when running + if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) { + return; + } + + // Get the actuators + struct pfc_actuator_t *act = &pfc_acts[pfc_actuators.act_idx]; + + // Verify the result and continue + if((get_sys_time_float() - pfc_actuators.act_start_time) > act->timeout) { + switch(pfc_actuators.act_state) { + case PFC_ACTUATOR_STATE_WAIT: + pfc_actuators_debug("Actuator %d starting LOW", pfc_actuators.act_idx); + pfc_actuators.act_state = PFC_ACTUATOR_STATE_LOW; + pfc_actuators.act_start_time = get_sys_time_float(); + pfc_actuators.last_feedback = NAN; + pfc_actuators.last_feedback2 = NAN; + break; + 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))) { + 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))) { + 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; + } + if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) { + pfc_actuators_debug("Actuator %d LOW ok %.2f, %.2f starting HIGH", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2); + pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING; + pfc_actuators.act_state = PFC_ACTUATOR_STATE_HIGH; + pfc_actuators.act_start_time = get_sys_time_float(); + pfc_actuators.last_feedback = NAN; + pfc_actuators.last_feedback2 = NAN; + } + break; + 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))) { + 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))) { + 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; + } + if(pfc_actuators.state != PFC_ACTUATORS_STATE_ERROR || PFC_ACTUATORS_DEBUG) { + pfc_actuators_debug("Actuator %d HIGH ok %.2f, %.2f", pfc_actuators.act_idx, pfc_actuators.last_feedback, pfc_actuators.last_feedback2); + pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING; + pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT; + pfc_actuators.act_start_time = 0; + pfc_actuators.act_idx++; + } + break; + } + } + + // Finished testing + if(pfc_actuators.act_idx >= pfc_actuators.act_nb) { + pfc_actuators.state = PFC_ACTUATORS_STATE_SUCCESS; + pfc_actuators_error("Actuators checks done %d", pfc_actuators.act_idx); + } +} + +/** + * @brief Start the actuator testing +*/ +void pfc_actuators_start(bool start) { + if(start && pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) { + pfc_actuators.act_idx = 0; + pfc_actuators.act_start_time = 0; + pfc_actuators.state = PFC_ACTUATORS_STATE_RUNNING; + pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT; + pfc_actuators.last_feedback = NAN; + pfc_actuators.last_feedback2 = NAN; + } + else if(!start && pfc_actuators.state == PFC_ACTUATORS_STATE_RUNNING) { + pfc_actuators.act_idx = 0; + pfc_actuators.act_start_time = 0; + pfc_actuators.state = PFC_ACTUATORS_STATE_INIT; + pfc_actuators.act_state = PFC_ACTUATOR_STATE_WAIT; + } +} + +/** + * @brief Get the actuator value in the command laws to move the actuator during the preflight checks + * @param idx The index of the actuator in the preflight checks struct + * @param value The value if no checks are performed on this actuator +*/ +int16_t pfc_actuators_value(uint8_t idx, int16_t value) { + if(idx != pfc_actuators.act_idx || pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING) { + return value; + } + + if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_LOW) { + return pfc_acts[pfc_actuators.act_idx].low; + } + else if(pfc_actuators.act_state == PFC_ACTUATOR_STATE_HIGH) { + return pfc_acts[pfc_actuators.act_idx].high; + } + + return value; +} + +/** + * @brief Check the actuators with feedback + * + * @param result The result of the preflight checks + */ +static void pfc_actuators_cb(struct preflight_result_t *result) +{ + switch(pfc_actuators.state) { + case PFC_ACTUATORS_STATE_INIT: + preflight_error(result, "Actuators not checked perform checks first[%d]", pfc_actuators.act_nb); + break; + case PFC_ACTUATORS_STATE_RUNNING: + preflight_error(result, "Actuators are currently being checked[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb); + break; + case PFC_ACTUATORS_STATE_SUCCESS: + preflight_success(result, "Actuators checked and moved succesfully[%d]", pfc_actuators.act_nb); + break; + case PFC_ACTUATORS_STATE_ERROR: + preflight_error(result, "Actuators not responding correctly[%d/%d]",pfc_actuators.act_idx, pfc_actuators.act_nb); + break; + } +} + +/** + * @brief Callback for the actuator feedback +*/ +static void pfc_act_feedback_cb(uint8_t sender_id __attribute__((unused)), struct act_feedback_t *feedback, uint8_t num_act) { + if(pfc_actuators.state != PFC_ACTUATORS_STATE_RUNNING || pfc_actuators.act_idx >= pfc_actuators.act_nb) { + return; + } + + // Save the last feedback values + for(uint8_t i = 0; i < num_act; i++) { + if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.rpm) { + pfc_actuators.last_feedback = feedback[i].rpm; + pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_RPM_ERROR; + } else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id && feedback[i].set.position) { + pfc_actuators.last_feedback = feedback[i].position; + pfc_actuators.last_feedback_err = PFC_ACTUATORS_MAX_ANGLE_ERROR; + } + + if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.rpm) { + pfc_actuators.last_feedback2 = feedback[i].rpm; + pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_RPM_ERROR; + } else if(feedback[i].idx == pfc_acts[pfc_actuators.act_idx].feedback_id2 && feedback[i].set.position) { + pfc_actuators.last_feedback2 = feedback[i].position; + pfc_actuators.last_feedback_err2 = PFC_ACTUATORS_MAX_ANGLE_ERROR; + } + } +} diff --git a/sw/airborne/modules/checks/pfc_actuators.h b/sw/airborne/modules/checks/pfc_actuators.h new file mode 100644 index 0000000000..491c5aaab2 --- /dev/null +++ b/sw/airborne/modules/checks/pfc_actuators.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2023 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 + * . + */ + +/** + * @file "modules/checks/pfc_actuators.h" + * @author Freek van Tienen + * Checks the actuators with feedback before takeoff + */ + +#ifndef PFC_ACTUATORS_H +#define PFC_ACTUATORS_H + +#include "std.h" + +extern void pfc_actuators_init(void); +extern void pfc_actuators_run(void); +extern void pfc_actuators_start(bool start); +extern int16_t pfc_actuators_value(uint8_t idx, int16_t value); + +#endif /* PFC_ACTUATORS_H */ diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index fd86ab79a5..ca3ef04877 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -164,14 +164,25 @@ let print_reverse_servo_table = fun out driver servos -> ) servos; fprintf out " default: return 0;\n"; fprintf out " };\n"; + fprintf out "}\n\n"; + fprintf out "static inline int get_servo_idx%s(int _idx) {\n" d; + fprintf out " switch (_idx) {\n"; + List.iter (fun c -> + let name = ExtXml.attrib c "name" in + fprintf out " case SERVO_%s_DRIVER_NO: return SERVO_%s_IDX;\n" name name; + ) servos; + fprintf out " default: return 0;\n"; + fprintf out " };\n"; fprintf out "}\n\n" let parse_servo = fun out driver c -> let shortname = ExtXml.attrib c "name" in let name = "SERVO_"^shortname and no_servo = int_of_string (ExtXml.attrib c "no") in + let global_idx = Hashtbl.length servos_drivers in define_out out (name^"_DRIVER_NO") (string_of_int no_servo); + define_out out (name^"_IDX") (string_of_int global_idx); let s_min = fos (ExtXml.attrib c "min" ) and neutral = fos (ExtXml.attrib c "neutral") @@ -194,7 +205,6 @@ let parse_servo = fun out driver c -> fprintf out "\n"; (* Memorize the associated driver (if any) and global index (insertion order) *) - let global_idx = Hashtbl.length servos_drivers in Hashtbl.add servos_drivers shortname (driver, global_idx) (* Characters checked in Gen_radio.checl_function_name *) @@ -207,7 +217,6 @@ let preprocess_value = fun s v prefix -> let print_actuators_idx = fun out -> Hashtbl.iter (fun s (d, i) -> - fprintf out "#define SERVO_%s_IDX %d\n" s i; (* Set servo macro *) fprintf out "#define Set_%s_Servo(_v) { \\\n" s; fprintf out " actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s; @@ -315,10 +324,8 @@ let rec parse_section = fun out ac_id s -> let driver = ExtXml.attrib_or_default s "driver" "Default" in let servos = Xml.children s in let nb_servos = List.fold_right (fun s m -> max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in - let servos_offset = Hashtbl.length servos_drivers in define_out out (sprintf "SERVOS_%s_NB" (String.uppercase_ascii driver)) (string_of_int nb_servos); - define_out out (sprintf "SERVOS_%s_OFFSET" (String.uppercase_ascii driver)) (string_of_int servos_offset); fprintf out "#include \"modules/actuators/actuators_%s.h\"\n" (String.lowercase_ascii driver); fprintf out "\n"; List.iter (parse_servo out driver) servos;