diff --git a/conf/modules/system_core.xml b/conf/modules/system_core.xml index aed9d8e3b8..be26138c33 100644 --- a/conf/modules/system_core.xml +++ b/conf/modules/system_core.xml @@ -9,7 +9,7 @@ - mcu,math,state_interface,@actuators|@intermcu,settings|@no_settings,@datalink,@telemetry + mcu,threads,math,state_interface,@actuators|@intermcu,settings|@no_settings,@datalink,@telemetry core
diff --git a/conf/modules/thd_test.xml b/conf/modules/thd_test.xml new file mode 100644 index 0000000000..e10cf106e2 --- /dev/null +++ b/conf/modules/thd_test.xml @@ -0,0 +1,37 @@ + + + + + A module to test threads functionnalities, like threads, mutexes, semaphores and so on. + A thread follow the syracuse suit, cadenced by the main ap thread from a periodic + function signaling a binary semaphore. + The counter thread and the AP thread exchange data protected by a mutex. + The counter thread is joined by the AP thread when the suit reach 1. + The counter thread can be restarted by setting the initial value from the settings: Threads test/thd_test_start_value + + + + + + + + + + + + threads + +
+ +
+ + + + + + + + + + + diff --git a/conf/modules/threads.xml b/conf/modules/threads.xml new file mode 100644 index 0000000000..164d15e9fb --- /dev/null +++ b/conf/modules/threads.xml @@ -0,0 +1,20 @@ + + + + + + Provide threads and synchronization primitives. + + + + +
+ +
+ + + + + + + diff --git a/sw/airborne/arch/chibios/modules/core/threads_arch.c b/sw/airborne/arch/chibios/modules/core/threads_arch.c new file mode 100644 index 0000000000..ddd093e674 --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/threads_arch.c @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ +#include "modules/core/threads.h" +#include "modules/core/threads_arch.h" +#include "stdbool.h" + + +int pprz_mtx_init(pprz_mutex_t* mtx) { + chMtxObjectInit(&mtx->mtx); + return 0; +} + +int pprz_mtx_lock(pprz_mutex_t* mtx) { + chMtxLock(&mtx->mtx); + return 0; +} + +int pprz_mtx_trylock(pprz_mutex_t* mtx) { + return chMtxTryLock(&mtx->mtx) ? 0 : -1; +} + +int pprz_mtx_unlock(pprz_mutex_t* mtx) { + chMtxUnlock(&mtx->mtx); + return 0; +} + + + +void pprz_bsem_init(pprz_bsem_t* bsem, bool taken) { + chBSemObjectInit(&bsem->bsem, taken); +} + +void pprz_bsem_wait(pprz_bsem_t* bsem) { + chBSemWait(&bsem->bsem); +} + +void pprz_bsem_signal(pprz_bsem_t* bsem) { + chBSemSignal(&bsem->bsem); +} + + +int pprz_thread_create(pprz_thread_t* thread, size_t size, const char *name, uint8_t prio, void (*pf)(void *), void* arg) { + *thread = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(size), name, prio, pf, arg); + if(*thread == NULL) { + return -1; + } + return 0; +} + +void pprz_thread_exit(void *retval) { + chThdExit((msg_t)retval); +} + + +int pprz_thread_join(pprz_thread_t* thread, void** retval) { + *retval = (void*)chThdWait(*thread); + return 0; +} + + +int pprz_thread_tryjoin(pprz_thread_t* thread, void** retval) { + if(chThdTerminatedX(*thread)) { + return pprz_thread_join(thread, retval); + } + return -1; +} diff --git a/sw/airborne/arch/chibios/modules/core/threads_arch.h b/sw/airborne/arch/chibios/modules/core/threads_arch.h new file mode 100644 index 0000000000..670d6930aa --- /dev/null +++ b/sw/airborne/arch/chibios/modules/core/threads_arch.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#pragma once + +#include + +#define PPRZ_NORMAL_PRIO NORMALPRIO + +#define THREADS_ATTRIBUTES + +struct pprzMutex { + mutex_t mtx; +}; + +struct pprzBSem { + binary_semaphore_t bsem; +}; + + +typedef thread_t* pprz_thread_t; diff --git a/sw/airborne/arch/linux/modules/core/threads_arch.c b/sw/airborne/arch/linux/modules/core/threads_arch.c new file mode 100644 index 0000000000..a4c1e22de7 --- /dev/null +++ b/sw/airborne/arch/linux/modules/core/threads_arch.c @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ +#include "modules/core/threads.h" +#include "modules/core/threads_arch.h" +#include "stdbool.h" +#include +#include + + +int pprz_mtx_init(pprz_mutex_t* mtx) { + return pthread_mutex_init(&mtx->mtx, NULL); +} + +int pprz_mtx_lock(pprz_mutex_t* mtx) { + return pthread_mutex_lock(&mtx->mtx); +} + +int pprz_mtx_trylock(pprz_mutex_t* mtx) { + return pthread_mutex_trylock(&mtx->mtx); +} + +int pprz_mtx_unlock(pprz_mutex_t* mtx) { + return pthread_mutex_unlock(&mtx->mtx); +} + + + +void pprz_bsem_init(pprz_bsem_t* bsem, bool taken) { + pthread_mutex_init(&bsem->mtx, NULL); + sem_init(&bsem->sem, 0, taken ? 0: 1); +} + +void pprz_bsem_wait(pprz_bsem_t* bsem) { + sem_wait(&bsem->sem); +} + +void pprz_bsem_signal(pprz_bsem_t* bsem) { + int val; + pthread_mutex_lock(&bsem->mtx); + if(!sem_getvalue(&bsem->sem, &val)) { + if(val < 1) { + sem_post(&bsem->sem); + } + } + pthread_mutex_unlock(&bsem->mtx); +} + + + +// stub function argument: the thread function to run with its argument. +struct stub_arg +{ + void (*func)(void*); + void* arg; +}; + +// stub function, ran in a new thread +static void* stub(void* arg) { + struct stub_arg* sa = (struct stub_arg*) arg; + sa->func(sa->arg); + free(sa); + return NULL; +} + +int pprz_thread_create(pprz_thread_t* thread, size_t size, const char *name, uint8_t prio, void (*func)(void*), void* arg) { + (void)size; + (void)prio; + struct stub_arg *sa = malloc(sizeof(struct stub_arg)); // allocate a stub arg. Must be allocated on the heap + sa->func = func; + sa->arg = arg; + int ret = pthread_create(thread, NULL, stub, (void*)sa); // launch the stub in a new thread + if(ret) {return ret;} + return pthread_setname_np(*thread, name); +} + +void pprz_thread_exit(void *retval) { + pthread_exit(retval); +} + +int pprz_thread_join(pprz_thread_t* thread, void** retval) { + return pthread_join(*thread, retval); +} + +int pprz_thread_tryjoin(pprz_thread_t* thread, void** retval) { + return pthread_tryjoin_np(*thread, retval); +} diff --git a/sw/airborne/arch/linux/modules/core/threads_arch.h b/sw/airborne/arch/linux/modules/core/threads_arch.h new file mode 100644 index 0000000000..896f4912ce --- /dev/null +++ b/sw/airborne/arch/linux/modules/core/threads_arch.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#pragma once + +#include +#include + +#define PPRZ_NORMAL_PRIO 128 + +#define THREADS_ATTRIBUTES + +struct pprzMutex { + pthread_mutex_t mtx; +}; + +struct pprzBSem { + pthread_mutex_t mtx; + sem_t sem; +}; + + +typedef pthread_t pprz_thread_t; diff --git a/sw/airborne/arch/sim/modules/core/threads_arch.c b/sw/airborne/arch/sim/modules/core/threads_arch.c new file mode 120000 index 0000000000..60ceeec516 --- /dev/null +++ b/sw/airborne/arch/sim/modules/core/threads_arch.c @@ -0,0 +1 @@ +../../../linux/modules/core/threads_arch.c \ No newline at end of file diff --git a/sw/airborne/arch/sim/modules/core/threads_arch.h b/sw/airborne/arch/sim/modules/core/threads_arch.h new file mode 120000 index 0000000000..d138d342ef --- /dev/null +++ b/sw/airborne/arch/sim/modules/core/threads_arch.h @@ -0,0 +1 @@ +../../../linux/modules/core/threads_arch.h \ No newline at end of file diff --git a/sw/airborne/arch/stm32/modules/core/threads_arch.c b/sw/airborne/arch/stm32/modules/core/threads_arch.c new file mode 100644 index 0000000000..fab9ba91a8 --- /dev/null +++ b/sw/airborne/arch/stm32/modules/core/threads_arch.c @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ +#include "modules/core/threads.h" +#include "modules/core/threads_arch.h" +#include "stdbool.h" + + +int pprz_mtx_init(pprz_mutex_t* mtx) { + (void)mtx; + return 0; +} + +int pprz_mtx_lock(pprz_mutex_t* mtx) { + (void)mtx; + return 0; +} + +int pprz_mtx_trylock(pprz_mutex_t* mtx) { + (void)mtx; + (void)mtx; + return 0; +} + +int pprz_mtx_unlock(pprz_mutex_t* mtx) { + (void)mtx; + return 0; +} + + + +void pprz_bsem_init(pprz_bsem_t* bsem, bool taken) { + bsem->value = taken ? 0: 1; +} + +void pprz_bsem_wait(pprz_bsem_t* bsem) { + while (bsem->value == 0) + { + // active wait + asm("NOP"); + } + bsem->value = 0; +} + +void pprz_bsem_signal(pprz_bsem_t* bsem) { + bsem->value = 1; +} diff --git a/sw/airborne/arch/stm32/modules/core/threads_arch.h b/sw/airborne/arch/stm32/modules/core/threads_arch.h new file mode 100644 index 0000000000..ceff0f05d7 --- /dev/null +++ b/sw/airborne/arch/stm32/modules/core/threads_arch.h @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#pragma once + +#define PPRZ_NORMAL_PRIO NORMALPRIO + +#define THREADS_ATTRIBUTES __attribute__((error("Threads cannot be used in STM32 bare metal ARCH."))) + +struct pprzMutex { + int dummy; // avoid warning: empty declaration +}; + +struct pprzBSem { + volatile int value; +}; + + +typedef int pprz_thread_t; diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index 2e74bdec70..46e9a49080 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -186,7 +186,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC) { // protect acces to fbw state - PPRZ_MUTEX_LOCK(fbw_state_mtx); + pprz_mtx_lock(&fbw_state_mtx); /*######################################## TILT CONTROL INPUT #############################################*/ #ifdef CAM_TILT_NEUTRAL @@ -266,7 +266,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, #endif //#ifdef CAM_PAN_NEUTRAL /*######################################## END OF PAN CONTROL INPUT #############################################*/ - PPRZ_MUTEX_UNLOCK(fbw_state_mtx); + pprz_mtx_unlock(&fbw_state_mtx); // Bound Pan and Tilt angles. if (cam_theta > RadOfDeg(CAM_TILT_MAX)) { diff --git a/sw/airborne/modules/core/threads.h b/sw/airborne/modules/core/threads.h new file mode 100644 index 0000000000..1feee7db7a --- /dev/null +++ b/sw/airborne/modules/core/threads.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2025 The Paparazzi Team + * + * 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 3 of the License, or + * (at your option) any later version. + * + * See LICENSE file for the full license version, or see http://www.gnu.org/licenses/ + */ + +#pragma once +#include "stdbool.h" +#include "modules/core/threads_arch.h" +#include "stdint.h" +#include + + +typedef struct pprzMutex pprz_mutex_t; +typedef struct pprzBSem pprz_bsem_t; + + +int pprz_mtx_init(pprz_mutex_t* mtx); +int pprz_mtx_lock(pprz_mutex_t* mtx); + +/** + * @brief Performs a nonblocking lock on the mutex. + * @return 0 if successful + */ +int pprz_mtx_trylock(pprz_mutex_t* mtx); +int pprz_mtx_unlock(pprz_mutex_t* mtx); + +void pprz_bsem_init(pprz_bsem_t* bsem, bool taken); +void pprz_bsem_wait(pprz_bsem_t* bsem); +void pprz_bsem_signal(pprz_bsem_t* bsem); + +/** + * @brief Creates a new thread whose stack is dynamically allocated. + * @param[in] size size of the stack. Unused on linux at the moment. + * @param[in] prio the priority level for the new thread. Unused on linux at the moment. NORMALPRIO + * @param[in] pf the thread function + * @param[in] arg an argument passed to the thread function. It can be + * @p NULL. + * @param[out] thread pointer to a pprz_thread_t that will be writen with the handle to the thread + * @return Return 0 on success, or a platform dependant error code. + * + * @api + */ +int pprz_thread_create(pprz_thread_t* thread, size_t size, const char *name, uint8_t prio, void (*func)(void*), void* arg) THREADS_ATTRIBUTES; + +/** + * @brief Exit the current thread + * @param[out] retval The thread's return value + */ +void pprz_thread_exit(void *retval) THREADS_ATTRIBUTES; + +/** + * @brief Wait for the thread to terminate. + * @note Warning ! This is a blocking function, do not use it in the main AP thread ! + * @param[in] thread The thread to be joined + * @param[out] retval The thread's return value will be written to the variable pointed by retval + */ +int pprz_thread_join(pprz_thread_t* thread, void** retval) THREADS_ATTRIBUTES; + + +/** + * @brief Performs a nonblocking join with the thread. + * @param[in] thread The thread to be joined + * @param[out] retval The thread's return value will be written to the variable pointed by retval. + * must not be used if this fuction did not returned 0. + * @return 0 if the thread successfully joined. + */ +int pprz_thread_tryjoin(pprz_thread_t* thread, void** retval) THREADS_ATTRIBUTES; diff --git a/sw/airborne/modules/datalink/bitcraze/syslink_dl.c b/sw/airborne/modules/datalink/bitcraze/syslink_dl.c index 6bd8008330..7c2a17cbc0 100644 --- a/sw/airborne/modules/datalink/bitcraze/syslink_dl.c +++ b/sw/airborne/modules/datalink/bitcraze/syslink_dl.c @@ -30,12 +30,12 @@ #include "mcu_periph/uart.h" #include #include "led.h" +#include "modules/core/threads.h" struct syslink_dl syslink; /** Protect syslink TX with Mutex when using RTOS */ -#include "pprz_mutex.h" -PPRZ_MUTEX(syslink_tx_mtx); +pprz_mutex_t syslink_tx_mtx; /** Send a syslink message */ @@ -220,7 +220,7 @@ static void handle_raw(syslink_message_t *msg) // send next raw message if fifo is not empty if (syslink.tx_extract_idx != syslink.tx_insert_idx) { - PPRZ_MUTEX_LOCK(syslink_tx_mtx); + pprz_mtx_lock(&syslink_tx_mtx); syslink_message_t msg_raw; msg_raw.type = SYSLINK_RADIO_RAW; memcpy(&msg_raw.length, &syslink.msg_tx[syslink.tx_extract_idx], sizeof(crtp_message_t)); @@ -230,7 +230,7 @@ static void handle_raw(syslink_message_t *msg) if (syslink.tx_extract_idx == CRTP_BUF_LEN) { syslink.tx_extract_idx = 0; } - PPRZ_MUTEX_UNLOCK(syslink_tx_mtx); + pprz_mtx_unlock(&syslink_tx_mtx); } } @@ -293,7 +293,7 @@ static int syslink_check_free_space(struct syslink_dl *s, long *fd UNUSED, uint1 } int space = CRTP_MAX_DATA_SIZE * (slots - 1); if (space >= len) { - PPRZ_MUTEX_LOCK(syslink_tx_mtx); + pprz_mtx_lock(&syslink_tx_mtx); return space; } return 0; @@ -342,7 +342,7 @@ static void syslink_put_byte(struct syslink_dl *s, long fd, const uint8_t b) // send_message is not needed as messages are stored in a fifo static void syslink_send_message(struct syslink_dl *s UNUSED, long fd UNUSED) { - PPRZ_MUTEX_UNLOCK(syslink_tx_mtx); // release mutex + pprz_mtx_lock(&syslink_tx_mtx); // release mutex } static uint8_t syslink_getch(struct syslink_dl *s) @@ -390,7 +390,7 @@ void syslink_dl_init(void) syslink.device.get_byte = (get_byte_t) syslink_getch; // init mutex if needed - PPRZ_MUTEX_INIT(syslink_tx_mtx); + pprz_mtx_init(&syslink_tx_mtx); } /** Periodic function */ diff --git a/sw/airborne/modules/datalink/gec_dl.c b/sw/airborne/modules/datalink/gec_dl.c index 6e547298d6..0fa2f32e37 100644 --- a/sw/airborne/modules/datalink/gec_dl.c +++ b/sw/airborne/modules/datalink/gec_dl.c @@ -144,7 +144,7 @@ static void start_message(struct pprzlink_msg *msg, long fd __attribute__((unused)), uint8_t payload_len __attribute__((unused))) { - PPRZ_MUTEX_LOCK(get_trans(msg)->mtx_tx); // lock mutex + pprz_mtx_lock(&get_trans(msg)->mtx_tx); // lock mutex memset(get_trans(msg)->tx_msg, 0, TRANSPORT_PAYLOAD_LEN);// erase message data get_trans(msg)->tx_msg_idx = 0;// reset index } @@ -205,7 +205,7 @@ static void end_message(struct pprzlink_msg *msg, long fd) break; } // unlock mutex - PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx); + pprz_mtx_unlock(&get_trans(msg)->mtx_tx); } /** @@ -219,7 +219,7 @@ void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd) get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, fd, DL_TYPE_UINT8, DL_FORMAT_SCALAR, get_trans(msg)->tx_msg, get_trans(msg)->tx_msg_idx); // unlock mutex - PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx); + pprz_mtx_unlock(&get_trans(msg)->mtx_tx); get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd); } @@ -263,7 +263,7 @@ static void start_message(struct gec_transport *trans, long fd __attribute__((unused)), uint8_t payload_len __attribute__((unused))) { - PPRZ_MUTEX_LOCK(trans->mtx_tx); // lock mutex + pprz_mtx_lock(&trans->mtx_tx); // lock mutex memset(trans->tx_msg, 0, TRANSPORT_PAYLOAD_LEN); // erase message data trans->tx_msg_idx = 0; // reset index } @@ -283,7 +283,7 @@ static void end_message(struct gec_transport *trans, struct link_device *dev, break; } // unlock mutex - PPRZ_MUTEX_UNLOCK(trans->mtx_tx); + pprz_mtx_unlock(&trans->mtx_tx); } void gec_encapsulate_and_send_msg(struct gec_transport *trans, @@ -293,7 +293,7 @@ void gec_encapsulate_and_send_msg(struct gec_transport *trans, trans->pprz_tp.trans_tx.put_bytes(trans, dev, fd, DL_TYPE_UINT8, DL_FORMAT_SCALAR, trans->tx_msg, trans->tx_msg_idx); // unlock mutex - PPRZ_MUTEX_UNLOCK(trans->mtx_tx); + pprz_mtx_unlock(&trans->mtx_tx); trans->pprz_tp.trans_tx.end_message(trans, dev, fd); } @@ -335,7 +335,7 @@ void gec_transport_init(struct gec_transport *t) t->trans_tx.overrun = (overrun_t) overrun; t->trans_tx.count_bytes = (count_bytes_t) count_bytes; t->trans_tx.impl = (void *)(t); - PPRZ_MUTEX_INIT(t->mtx_tx); // init mutex, check if correct pointer + pprz_mtx_init(&t->mtx_tx); // init mutex, check if correct pointer // add whitelist messages gec_add_to_whitelist(&(t->whitelist), KEY_EXCHANGE_MSG_ID_UAV); diff --git a/sw/airborne/modules/datalink/gec_dl.h b/sw/airborne/modules/datalink/gec_dl.h index b8cf43790c..82b239c53b 100644 --- a/sw/airborne/modules/datalink/gec_dl.h +++ b/sw/airborne/modules/datalink/gec_dl.h @@ -29,7 +29,7 @@ #include "pprzlink/pprzlink_transport.h" #include "pprzlink/pprz_transport.h" #include "modules/datalink/gec/gec.h" -#include "pprz_mutex.h" +#include "modules/core/threads.h" #include "mcu_periph/uart.h" #if USE_USB_SERIAL || USE_USB_SERIAL_DEBUG @@ -84,7 +84,7 @@ struct gec_transport { struct gec_whitelist whitelist; // optional mutex - PPRZ_MUTEX(mtx_tx); + pprz_mutex_t mtx_tx; }; /** PPRZ transport structure */ diff --git a/sw/airborne/modules/mission/copilot.h b/sw/airborne/modules/mission/copilot.h index b15e7ed34e..b48058549f 100644 --- a/sw/airborne/modules/mission/copilot.h +++ b/sw/airborne/modules/mission/copilot.h @@ -46,11 +46,12 @@ #include "modules/datalink/datalink.h" #include "modules/datalink/extra_pprz_dl.h" -#include "pprz_mutex.h" +#include "modules/core/threads.h" + +extern pprz_mutex_t copilot_cam_snapshot_mtx; +extern pprz_mutex_t copilot_cam_payload_mtx; +extern pprz_mutex_t copilot_status_mtx; -PPRZ_MUTEX_DECL(copilot_cam_snapshot_mtx); -PPRZ_MUTEX_DECL(copilot_cam_payload_mtx); -PPRZ_MUTEX_DECL(copilot_status_mtx); struct CameraPayload { float timestamp; diff --git a/sw/airborne/modules/mission/copilot_common.c b/sw/airborne/modules/mission/copilot_common.c index 9fc221c5bb..fcdefd9ac2 100644 --- a/sw/airborne/modules/mission/copilot_common.c +++ b/sw/airborne/modules/mission/copilot_common.c @@ -52,9 +52,9 @@ struct CameraPayload cam_payload; struct CameraSnapshot cam_snapshot; struct CopilotStatus copilot_status; -PPRZ_MUTEX(copilot_cam_snapshot_mtx); -PPRZ_MUTEX(copilot_cam_payload_mtx); -PPRZ_MUTEX(copilot_status_mtx); +pprz_mutex_t copilot_cam_snapshot_mtx; +pprz_mutex_t copilot_cam_payload_mtx; +pprz_mutex_t copilot_status_mtx; /** Init function */ void copilot_init(void) @@ -67,15 +67,15 @@ void copilot_init(void) memset(&cam_snapshot, 0, sizeof(cam_snapshot)); memset(&copilot_status, 0, sizeof(copilot_status)); - PPRZ_MUTEX_INIT(copilot_cam_snapshot_mtx); - PPRZ_MUTEX_INIT(copilot_cam_payload_mtx); - PPRZ_MUTEX_INIT(copilot_status_mtx); + pprz_mtx_init(&copilot_cam_snapshot_mtx); + pprz_mtx_init(&copilot_cam_payload_mtx); + pprz_mtx_init(&copilot_status_mtx); } /** Periodic function */ void copilot_periodic(void) { - PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx); + pprz_mtx_lock(&copilot_cam_snapshot_mtx); if (send_cam_snapshot) { // send down to GCS DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice, @@ -85,9 +85,9 @@ void copilot_periodic(void) send_cam_snapshot = false; } - PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx); + pprz_mtx_unlock(&copilot_cam_snapshot_mtx); - PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx); + pprz_mtx_lock(&copilot_cam_payload_mtx); if (send_cam_payload) { // NOTE: to send the message over the EXTRA_DL port // use "DOWNLINK_SEND_CAMERA_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE," @@ -99,9 +99,9 @@ void copilot_periodic(void) send_cam_payload = false; } - PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx); + pprz_mtx_unlock(&copilot_cam_payload_mtx); - PPRZ_MUTEX_LOCK(copilot_status_mtx); + pprz_mtx_lock(&copilot_status_mtx); // send down to GCS if (send_copilot_status) { DOWNLINK_SEND_COPILOT_STATUS(DefaultChannel, DefaultDevice, @@ -111,7 +111,7 @@ void copilot_periodic(void) send_copilot_status = false; } - PPRZ_MUTEX_UNLOCK(copilot_status_mtx); + pprz_mtx_unlock(&copilot_status_mtx); } @@ -126,7 +126,7 @@ void copilot_periodic(void) */ void copilot_parse_cam_snapshot_dl(uint8_t *buf) { - PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx); + pprz_mtx_lock(&copilot_cam_snapshot_mtx); // copy CAMERA_SNAPSHOT message and mark it to be sent cam_snapshot.cam_id = DL_CAMERA_SNAPSHOT_DL_camera_id(buf); @@ -138,7 +138,7 @@ void copilot_parse_cam_snapshot_dl(uint8_t *buf) send_cam_snapshot = true; - PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx); + pprz_mtx_unlock(&copilot_cam_snapshot_mtx); } /** @@ -146,7 +146,7 @@ void copilot_parse_cam_snapshot_dl(uint8_t *buf) */ void copilot_parse_cam_payload_dl(uint8_t *buf) { - PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx); + pprz_mtx_lock(&copilot_cam_payload_mtx); cam_payload.timestamp = DL_CAMERA_PAYLOAD_DL_timestamp(buf); cam_payload.used_mem = DL_CAMERA_PAYLOAD_DL_used_memory(buf); @@ -156,7 +156,7 @@ void copilot_parse_cam_payload_dl(uint8_t *buf) send_cam_payload = true; - PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx); + pprz_mtx_unlock(&copilot_cam_payload_mtx); } /** @@ -164,7 +164,7 @@ void copilot_parse_cam_payload_dl(uint8_t *buf) */ void copilot_parse_copilot_status_dl(uint8_t *buf) { - PPRZ_MUTEX_LOCK(copilot_status_mtx); + pprz_mtx_lock(&copilot_status_mtx); copilot_status.timestamp = DL_COPILOT_STATUS_DL_timestamp(buf); copilot_status.used_mem = DL_COPILOT_STATUS_DL_used_memory(buf); @@ -174,7 +174,7 @@ void copilot_parse_copilot_status_dl(uint8_t *buf) send_copilot_status = true; - PPRZ_MUTEX_UNLOCK(copilot_status_mtx); + pprz_mtx_unlock(&copilot_status_mtx); } /** diff --git a/sw/airborne/modules/sensors/thd_test.c b/sw/airborne/modules/sensors/thd_test.c new file mode 100644 index 0000000000..1ee19ef0c2 --- /dev/null +++ b/sw/airborne/modules/sensors/thd_test.c @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2025 fab + * + * 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/sensors/thd_test.c" + * @author fab + * A module to test threads functionnalities, like threads, mutexes, semaphores and so on. + */ + +#include "modules/sensors/thd_test.h" +#include "modules/core/threads.h" +#include "led.h" +#include "modules/datalink/telemetry.h" +#include + +float thd_test_start_value; + +pprz_bsem_t bsem; + +pprz_mutex_t mtx; // mutex protecting syracuse +static int syracuse = 14; + +// Remember if the thread has already been joined or not. +// From https://linux.die.net/man/3/pthread_join: +// "Joining with a thread that has previously been joined results in undefined behavior." +static bool joined = true; + +static void test_thd(void*); + +pprz_thread_t thd_handle; + +void thd_test_syracuse_restart(float s) { + // try changing syracuse + // non blocking because we are in the main loop. + // If it doesn't work, just try again! + if(!pprz_mtx_trylock(&mtx)) { + syracuse = (int)s; + thd_test_start_value = s; + pprz_mtx_unlock(&mtx); + + // if it was stopped, restart it! + if(joined) { + if(!pprz_thread_create(&thd_handle, 512, "test", PPRZ_NORMAL_PRIO+1, test_thd, NULL)) { + joined = false; + } + } + } else { + char err[] = "Could not lock"; + DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, strlen(err), err); + } +} + +void thd_test_init(void) +{ + pprz_bsem_init(&bsem, true); // init binary semaphore + pprz_mtx_init(&mtx); // init mutex + // create thread, and remember to try joining it. + if(!pprz_thread_create(&thd_handle, 512, "test", PPRZ_NORMAL_PRIO, test_thd, NULL)) { + joined = false; + } +} + +void thd_test_periodic(void) +{ + if(!joined) { + size_t ret; + char msg[30]; + size_t len = 0; + if(!pprz_thread_tryjoin(&thd_handle, (void**)(&ret))) { + // try joining the thread. If successful, send its exit value. + len = snprintf(msg, sizeof(msg), "Finished in %zu steps!", ret); + joined = true; + } else { + // the thread is still running, print the current value of the syracuse suit (mutex protected) + // then signal the binary semaphore to allow the thread to resume execution + if(!pprz_mtx_trylock(&mtx)) { + len = snprintf(msg, sizeof(msg), "%d", syracuse); + pprz_mtx_unlock(&mtx); + pprz_bsem_signal(&bsem); + } + } + // send the message to the ground (current value or end message) + if(len > 0) { + DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, len, msg); + } + + + } + +} + + +static void test_thd(void* arg) { + (void)arg; + size_t count = 0; + while(syracuse != 1) { + pprz_bsem_wait(&bsem); // wait to be woken up by the AP thread + + pprz_mtx_lock(&mtx); + if(syracuse%2) { + syracuse = 3*syracuse + 1; + } else { + syracuse = syracuse / 2; + } + pprz_mtx_unlock(&mtx); + count++; + } + + pprz_thread_exit((void*)((size_t)count)); +} diff --git a/sw/airborne/modules/sensors/thd_test.h b/sw/airborne/modules/sensors/thd_test.h new file mode 100644 index 0000000000..5f56394c28 --- /dev/null +++ b/sw/airborne/modules/sensors/thd_test.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2025 fab + * + * 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/sensors/thd_test.h" + * @author fab + * A module to test threads functionnalities, like threads, mutexes, semaphores and so on. + */ + +#ifndef THD_TEST_H +#define THD_TEST_H + +extern void thd_test_init(void); +extern void thd_test_periodic(void); +extern void thd_test_event(void); + +void thd_test_syracuse_restart(float s); + +extern float thd_test_start_value; + +#endif // THD_TEST_H diff --git a/sw/airborne/pprz_mutex.h b/sw/airborne/pprz_mutex.h deleted file mode 100644 index 9ef38375f3..0000000000 --- a/sw/airborne/pprz_mutex.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2016 Gautier Hattenberger - * - * 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 pprz_mutex.h - * - * Utility functions and macros to abstract some RTOS functionalities - * such as mutexes. - */ - -#ifndef PPRZ_MUTEX_H -#define PPRZ_MUTEX_H - -#if USE_CHIBIOS_RTOS - -#include - -#define PPRZ_MUTEX(_mtx) mutex_t _mtx -#define PPRZ_MUTEX_DECL(_mtx) extern mutex_t _mtx -#define PPRZ_MUTEX_INIT(_mtx) chMtxObjectInit(&(_mtx)) -#define PPRZ_MUTEX_LOCK(_mtx) chMtxLock(&(_mtx)) -#define PPRZ_MUTEX_UNLOCK(_mtx) chMtxUnlock(&(_mtx)) - -#else // no RTOS - -#define PPRZ_MUTEX(_mtx) -#define PPRZ_MUTEX_DECL(_mtx) -#define PPRZ_MUTEX_INIT(_mtx) {} -#define PPRZ_MUTEX_LOCK(_mtx) {} -#define PPRZ_MUTEX_UNLOCK(_mtx) {} - -#endif - -#endif - diff --git a/tests/modules/test_arch/modules/core/threads_arch.h b/tests/modules/test_arch/modules/core/threads_arch.h new file mode 100644 index 0000000000..9fcd75948b --- /dev/null +++ b/tests/modules/test_arch/modules/core/threads_arch.h @@ -0,0 +1,14 @@ +#pragma once + +#define PPRZ_NORMAL_PRIO 128 + +#define THREADS_ATTRIBUTES + +struct pprzMutex { +}; + +struct pprzBSem { +}; + + +typedef struct{} pprz_thread_t;