diff --git a/components/logging/cn_mavlink_stubs.h b/components/logging/cn_mavlink_stubs.h index 07a7f9e0..3aa94ab0 100644 --- a/components/logging/cn_mavlink_stubs.h +++ b/components/logging/cn_mavlink_stubs.h @@ -3,7 +3,60 @@ // CN-compatible stubs for the MAVLink functions we use. #include -#include + +// From `mavlink/mavlink_types.h` + +struct __mavlink_message { + // TODO: Make this an incomplete type. Currently, this placeholder + // definition is necessary because CN errors on incomplete types. + uint8_t _dummy; +}; +typedef struct __mavlink_message mavlink_message_t; + +const char* _mav_payload_impl(const mavlink_message_t* msg); +#define _MAV_PAYLOAD(msg) (_mav_payload_impl(msg)) + +typedef enum { + MAVLINK_TYPE_CHAR = 0, + MAVLINK_TYPE_UINT8_T = 1, + MAVLINK_TYPE_INT8_T = 2, + MAVLINK_TYPE_UINT16_T = 3, + MAVLINK_TYPE_INT16_T = 4, + MAVLINK_TYPE_UINT32_T = 5, + MAVLINK_TYPE_INT32_T = 6, + MAVLINK_TYPE_UINT64_T = 7, + MAVLINK_TYPE_INT64_T = 8, + MAVLINK_TYPE_FLOAT = 9, + MAVLINK_TYPE_DOUBLE = 10 +} mavlink_message_type_t; + +#define MAVLINK_MAX_FIELDS 64 + +typedef struct __mavlink_field_info { + const char *name; // name of this field + const char *print_format; // printing format hint, or NULL + mavlink_message_type_t type; // type of this field + unsigned int array_length; // if non-zero, field is an array + unsigned int wire_offset; // offset of each field in the payload + unsigned int structure_offset; // offset in a C structure +} mavlink_field_info_t; + +// note that in this structure the order of fields is the order +// in the XML file, not necessary the wire order +typedef struct __mavlink_message_info { + uint32_t msgid; // message ID + const char *name; // name of the message + unsigned num_fields; // how many fields in this message + mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information +} mavlink_message_info_t; + +struct __mavlink_status { + // TODO: Make this an incomplete type. Currently, this placeholder + // definition is necessary because CN errors on incomplete types. + uint8_t _dummy; +}; +typedef struct __mavlink_status mavlink_status_t; + // From `mavlink/mavlink_get_info.h` const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg); diff --git a/components/logging/cn_stubs.h b/components/logging/cn_stubs.h new file mode 100644 index 00000000..f8e2239c --- /dev/null +++ b/components/logging/cn_stubs.h @@ -0,0 +1,44 @@ +#pragma once + +// CN-compatible stubs for libc functions we use. + +#include + +#include +// Additional functions that are missing from CN's `time.h` +void tzset(void); +size_t strftime(char *s, size_t max, + const char *restrict format, const struct tm *restrict tm); +struct tm *localtime_r(const time_t *restrict timep, + struct tm *restrict result); + + +// From `stdio.h` + +#ifndef WAR_CN_309 +// not possible to call this due to CN issue #309 +// this spec isn't right but can't develop it at all without #309 +void perror(const char *msg); +/*$ spec perror(pointer msg); + requires take mi = Owned(msg); + ensures take mo = Owned(msg); + mi == mo; +$*/ +#else +# define perror(...) 0 +#endif + +#define printf(...) 0 +#define vsnprintf(...) 0 + + +// Helpers for dispatching based on number of arguments. Taken from +// https://stackoverflow.com/a/16683147 + +#define CAT( A, B ) A ## B +#define SELECT( NAME, NUM ) CAT( NAME ## _, NUM ) + +#define GET_COUNT( _1, _2, _3, _4, _5, _6 /* ad nauseam */, COUNT, ... ) COUNT +#define VA_SIZE( ... ) GET_COUNT( __VA_ARGS__, 6, 5, 4, 3, 2, 1 ) + +#define VA_SELECT( NAME, ... ) SELECT( NAME, VA_SIZE(__VA_ARGS__) )(__VA_ARGS__) diff --git a/components/logging/output.c b/components/logging/output.c index 3c281476..85ec220f 100644 --- a/components/logging/output.c +++ b/components/logging/output.c @@ -1,12 +1,13 @@ // Code for converting a MAVLink message into log output. -#include #include #include -#include #include +#include #ifndef CN_ENV +# include +# include // `mavlink_get_info.h` uses `offsetof`, but doesn't include the header // `stddef.h` that provides it. We include the header here so `offsetof` will // be available. @@ -14,6 +15,7 @@ # include # include #else +# include "cn_stubs.h" # include "cn_mavlink_stubs.h" #endif @@ -31,6 +33,9 @@ void buffer_init(struct buffer* b) { b->buf[0] = '\0'; } + +#ifndef CN_ENV + void buffer_printf(struct buffer* b, const char* fmt, ...) { va_list args; va_start(args, fmt); @@ -47,6 +52,37 @@ void buffer_printf(struct buffer* b, const char* fmt, ...) { va_end(args); } +#else + +// Dispatch by argument count to different variants of `buffer_printf`. +// +// Conveniently, we never call `buffer_printf` twice with the same number of +// arguments but with different argument types. If this changes, we could +// potentially handle it by making all variants take `void*` and wrap each +// variant in a macro that inserts the necessary casts. + +#define buffer_printf(...) VA_SELECT(buffer_printf, __VA_ARGS__) + +void buffer_printf_2(struct buffer* b, const char* fmt) { + // TODO +} + +void buffer_printf_3(struct buffer* b, const char* fmt, const char* arg0) { + // TODO +} + +void buffer_printf_4(struct buffer* b, const char* fmt, int arg0, const char* arg1) { + // TODO +} + +void buffer_printf_5(struct buffer* b, const char* fmt, + int arg0, const char* arg1, int64_t arg2) { + // TODO +} + +#endif + + void buffer_strftime(struct buffer* b, const char* fmt, const struct tm* tm) { size_t avail = sizeof(b->buf) - b->pos; size_t ret = strftime(b->buf + b->pos, avail, fmt, tm); @@ -131,12 +167,17 @@ void handle_message(const mavlink_message_t* msg) { *(int64_t*)(payload + field->wire_offset)); break; case MAVLINK_TYPE_FLOAT: +// CN doesn't support floats. +#ifndef CN_ENV buffer_printf(&buf, "%c %s=%f", delim, field->name, *(float*)(payload + field->wire_offset)); +#endif break; case MAVLINK_TYPE_DOUBLE: +#ifndef CN_ENV buffer_printf(&buf, "%c %s=%lf", delim, field->name, *(double*)(payload + field->wire_offset)); +#endif break; } } diff --git a/components/logging/output.h b/components/logging/output.h index e6f1a526..cd9e777f 100644 --- a/components/logging/output.h +++ b/components/logging/output.h @@ -1,5 +1,9 @@ #pragma once -#include +#ifndef CN_ENV +# include +#else +# include "cn_mavlink_stubs.h" +#endif void handle_message(const mavlink_message_t* msg);