Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #1440 from paparazzi/register_multiple_periodics

Browse Source 
Register multiple periodic messages

- Allow to add multiple callbacks: up to TELEMETRY_NB_CBS slots (4 right now).
- Also return slot index of callback (to make it easier to add an unregister function later) or -1 on failure
This commit is contained in:
Felix Ruess
2015-11-25 14:37:05 +01:00
parent bd713245c3 7b8aee102b
commit 3b5dd6131f
3 changed files with 46 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/datalink/telemetry.c
+18 -13
View File
@@ -30,10 +30,11 @@
#include "subsystems/datalink/telemetry_common.h"
#include "generated/periodic_telemetry.h"
/* Implement global structures from generated header
/* Implement global structures from generated header.
* Can register up to #TELEMETRY_NB_CBS callbacks per periodic message.
*/
telemetry_msg telemetry_msgs[TELEMETRY_NB_MSG] = TELEMETRY_MSG_NAMES;
telemetry_cb telemetry_cbs[TELEMETRY_NB_MSG] = TELEMETRY_CBS_NULL;
struct telemetry_cb_slots telemetry_cbs[TELEMETRY_NB_MSG] = TELEMETRY_CBS_NULL;
struct periodic_telemetry pprz_telemetry = { TELEMETRY_NB_MSG, telemetry_msgs, telemetry_cbs };
@@ -41,25 +42,29 @@ struct periodic_telemetry pprz_telemetry = { TELEMETRY_NB_MSG, telemetry_msgs, t
* @param _pt periodic telemetry structure to register
* @param _msg message name (string) as defined in telemetry xml file
* @param _cb callback function, called according to telemetry mode and specified period
* @return TRUE if message registered with success, FALSE otherwise
* @return -1 on failure to register, index of callback otherwise
*/
bool_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb)
{
// return FALSE if NULL is passed as periodic_telemetry
if (_pt == NULL) { return FALSE; }
// return if NULL is passed as periodic_telemetry
if (_pt == NULL) { return -1; }
// look for message name
uint8_t i;
uint8_t i, j;
for (i = 0; i < _pt->nb; i++) {
if (str_equal(_pt->msgs[i], _msg)) {
// register callback if not already done
if (_pt->cbs[i] == NULL) {
_pt->cbs[i] = _cb;
return TRUE;
} else { return FALSE; }
// register another callback if not all TELEMETRY_NB_CBS slots taken
for (j = 0; j < TELEMETRY_NB_CBS; j++) {
if (_pt->cbs[i].slots[j] == NULL) {
_pt->cbs[i].slots[j] = _cb;
return j;
}
}
// message matched but no more empty slots available
return -1;
}
}
// message name is not in telemetry file
return FALSE;
return -1;
}
#if USE_PERIODIC_TELEMETRY_REPORT
sw/airborne/subsystems/datalink/telemetry_common.h
+13 -7
View File
@@ -42,15 +42,21 @@ typedef void (*telemetry_cb)(struct transport_tx *trans, struct link_device *dev
*/
typedef const char telemetry_msg[64];
/** number of callbacks that can be registered per msg */
#define TELEMETRY_NB_CBS 4
struct telemetry_cb_slots {
telemetry_cb slots[TELEMETRY_NB_CBS];
};
/** Periodic telemetry structure.
* Contains the total number of messages (from generated telemetry file)
* and the list of registered callbacks
*/
struct periodic_telemetry {
uint8_t nb; ///< number of messages
telemetry_msg *msgs; ///< the array of msg names
telemetry_cb *cbs; ///< array of associated callbacks
uint8_t nb; ///< number of messages
telemetry_msg *msgs; ///< the array of msg names
struct telemetry_cb_slots *cbs; ///< array of associated callbacks
};
/** Register a telemetry callback function.
@@ -58,13 +64,13 @@ struct periodic_telemetry {
* @param _pt periodic telemetry structure to register
* @param _msg message name (string) as defined in telemetry xml file
* @param _cb callback function, called according to telemetry mode and specified period
* @return TRUE if message registered with success, FALSE otherwise
* @return -1 on failure to register, index of callback otherwise
*/
#if PERIODIC_TELEMETRY
extern bool_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb);
extern int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb);
#else
static inline bool_t register_periodic_telemetry(struct periodic_telemetry *_pt __attribute__((unused)),
const char *_msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return FALSE; }
static inline int8_t register_periodic_telemetry(struct periodic_telemetry *_pt __attribute__((unused)),
const char *_msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return -1; }
#endif
#if USE_PERIODIC_TELEMETRY_REPORT
sw/tools/generators/gen_periodic.ml
+15 -5
View File
@@ -65,6 +65,8 @@ let output_modes = fun out_h process_name modes freq modules ->
lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\n" _type v v v m v;
) modulos;
lprintf out_h "uint8_t j;\n";
(** For each message in this mode *)
let messages = List.sort (fun (_,p) (_,p') -> compare p p') messages in
let i = ref 0 in (** Basic balancing:1 message every 10Hz *)
@@ -82,12 +84,17 @@ let output_modes = fun out_h process_name modes freq modules ->
l := (p, !phase) :: !l;
i := !i + freq/10;
right ();
lprintf out_h "if (telemetry->cbs[TELEMETRY_MSG_%s_ID] != NULL)\n" message_name;
lprintf out_h "for (j = 0; j < TELEMETRY_NB_CBS; j++) {\n";
right ();
lprintf out_h "telemetry->cbs[TELEMETRY_MSG_%s_ID](trans, dev);\n" message_name;
lprintf out_h "if (telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j] != NULL)\n" message_name;
right ();
lprintf out_h "telemetry->cbs[TELEMETRY_MSG_%s_ID].slots[j](trans, dev);\n" message_name;
left ();
lprintf out_h "else break;\n";
left ();
lprintf out_h "}\n";
fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n";
lprintf out_h "else periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_MSG_%s_ID);\n" process_name process_name message_name;
lprintf out_h "if (j == 0) periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_MSG_%s_ID);\n" process_name process_name message_name;
fprintf out_h "#endif\n";
left ();
lprintf out_h "}\n"
@@ -155,8 +162,11 @@ let print_message_table = fun out_h xml ->
Hashtbl.iter (fun n _ -> fprintf out_h " \"%s\", \\\n" n) messages;
fprintf out_h "};\n\n";
fprintf out_h "#define TELEMETRY_CBS_NULL { \\\n";
for i = 1 to (Hashtbl.length messages) do fprintf out_h " NULL, \\\n" done;
fprintf out_h "};\n\n"
for i = 1 to (Hashtbl.length messages) do
(* use one 0 to init all slots (number TELEMETRY_NB_CBS) to NULL *)
fprintf out_h " {{ NULL }}, \\\n";
done;
fprintf out_h "}\n\n"
let print_process_send = fun out_h xml freq modules ->
(** For each process *)