From 401c9cf17cbf6d571a8f8652fe3022c8779d1677 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 23:35:52 +0200 Subject: Assorted hacks to at least compile on Mac OS, does not link yet --- makefiles/posix/config_posix_default.mk | 8 ++++---- makefiles/toolchain_native.mk | 15 ++++++++------- src/drivers/device/i2c_posix.cpp | 10 ++++++++++ .../attitude_estimator_ekf_main.cpp | 1 - src/modules/commander/accelerometer_calibration.cpp | 1 - src/modules/commander/commander.cpp | 4 ++++ src/modules/mavlink/mavlink_main_posix.cpp | 17 ++++++++++++++--- src/modules/mavlink/mavlink_receiver_posix.cpp | 7 ++++++- src/modules/systemlib/param/param.h | 6 +++--- src/platforms/posix/px4_layer/drv_hrt.c | 20 +++++++++++++++++++- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 2 +- 11 files changed, 69 insertions(+), 22 deletions(-) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 53b315346..6618faa5c 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -20,13 +20,13 @@ MODULES += systemcmds/param # # General system control # -MODULES += modules/mavlink +#MODULES += modules/mavlink # # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/ekf_att_pos_estimator +#MODULES += modules/ekf_att_pos_estimator # # Vehicle Control @@ -37,10 +37,10 @@ MODULES += modules/mc_att_control # Library modules # MODULES += modules/systemlib -MODULES += modules/systemlib/mixer +#MODULES += modules/systemlib/mixer MODULES += modules/uORB MODULES += modules/dataman -MODULES += modules/sdlog2 +#MODULES += modules/sdlog2 MODULES += modules/simulator MODULES += modules/commander diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 763b25ce8..b9a6a4dcc 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -120,7 +120,7 @@ ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ - -D__PX4_LINUX -D__PX4_POSIX \ + -D__PX4_POSIX \ -Dnoreturn_function= \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ @@ -206,7 +206,8 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) -EXTRA_LIBS += -pthread -lm -lrt +EXTRA_LIBS += -pthread -lm +#-lrt # Flags we pass to the C compiler # @@ -258,7 +259,7 @@ LDFLAGS += $(EXTRALDFLAGS) \ # Compiler support library # -LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) +LIBGCC := #$(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) # Files that the final link depends on # @@ -302,19 +303,19 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -o $1 $2 + $(Q) $(LD) -r -o $1 $2 endef # Produce partially-linked $1 from files in $2 # -#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +#$(Q) $(LD) -r -o $1 $2 # -Ur not supported in ld.gold define PRELINKF @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + $(Q) $(LD) -r -o $1 $2 endef -# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +# $(Q) $(LD) -r -o $1 $2 && $(OBJCOPY) --localize-hidden $1 # Update the archive $1 with the files in $2 # diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 51e8e2083..37671e26f 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -127,6 +127,7 @@ I2C::init() int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { + #ifdef __PX4_LINUX struct i2c_msg msgv[2]; unsigned msgs; struct i2c_rdwr_ioctl_data packets; @@ -183,11 +184,15 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } while (retry_count++ < _retries); return ret; + #else + return 1; + #endif } int I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { + #ifdef __PX4_LINUX struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; @@ -219,6 +224,9 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) } while (retry_count++ < _retries); return ret; + #else + return 1; + #endif } int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) @@ -226,9 +234,11 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; switch (cmd) { + #ifdef __PX4_LINUX case I2C_RDWR: warnx("Use I2C::transfer, not ioctl"); return 0; + #endif default: /* give it to the superclass */ return VDev::ioctl(handlep, cmd, arg); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 3a09b44bb..afca06588 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4248ad282..56360e986 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -132,7 +132,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 97873e462..0c8168243 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -54,7 +54,9 @@ #include #include //#include +#ifndef __PX4_POSIX #include +#endif #include #include #include @@ -2567,8 +2569,10 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul void *commander_low_prio_loop(void *arg) { + #ifndef __PX4_POSIX /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); + #endif /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index 784118916..b4dbd58d4 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -590,8 +590,16 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 230400: speed = B230400; break; + #ifndef B460800 + #define B460800 460800 + #endif + case 460800: speed = B460800; break; + #ifndef B921600 + #define B921600 921600 + #endif + case 921600: speed = B921600; break; default: @@ -729,9 +737,12 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; -// No FIONWRITE on Linux -#ifndef __PX4_LINUX - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +// No FIONWRITE on POSIX +#ifdef __PX4_POSIX + // fake a big buffer on larger systems + buf_free = 1000; +#else + (void) ioctl(_uart_fd, FIONWRITESIOCOUTQ, (unsigned long)&buf_free); #endif if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { diff --git a/src/modules/mavlink/mavlink_receiver_posix.cpp b/src/modules/mavlink/mavlink_receiver_posix.cpp index bfe7a4552..6e8e713de 100644 --- a/src/modules/mavlink/mavlink_receiver_posix.cpp +++ b/src/modules/mavlink/mavlink_receiver_posix.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -59,7 +59,9 @@ #include #include #include +#ifdef __PX4_LINUX #include +#endif #include #include #include @@ -1493,7 +1495,10 @@ MavlinkReceiver::receive_thread(void *arg) /* set thread name */ char thread_name[24]; sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + + #ifdef __PX4_LINUX prctl(PR_SET_NAME, thread_name, getpid()); + #endif px4_pollfd_struct_t fds[1]; fds[0].fd = uart_fd; diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index da810abea..e0515f137 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -340,7 +340,7 @@ __EXPORT int param_load_default(void); /** define an int32 parameter */ #define PARAM_DEFINE_INT32(_name, _default) \ static const \ - __attribute__((used, section("__param"))) \ + __attribute__((used, section(".paramsec,__param"))) \ struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_INT32, \ @@ -350,7 +350,7 @@ __EXPORT int param_load_default(void); /** define a float parameter */ #define PARAM_DEFINE_FLOAT(_name, _default) \ static const \ - __attribute__((used, section("__param"))) \ + __attribute__((used, section(".paramsec,__param"))) \ struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_FLOAT, \ @@ -360,7 +360,7 @@ __EXPORT int param_load_default(void); /** define a parameter that points to a structure */ #define PARAM_DEFINE_STRUCT(_name, _default) \ static const \ - __attribute__((used, section("__param"))) \ + __attribute__((used, section(".paramsec,__param"))) \ struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_STRUCT + sizeof(_default), \ diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 066abe70d..013c33982 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -41,6 +41,13 @@ #include #include +#include +#define ORWL_NANO (+1.0E-9) +#define ORWL_GIGA UINT64_C(1000000000) + +static double orwl_timebase = 0.0; +static uint64_t orwl_timestart = 0; + static struct sq_queue_s callout_queue; /* latency histogram */ @@ -61,7 +68,18 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); + // clock_gettime(CLOCK_MONOTONIC, &ts); + // be more careful in a multithreaded environement + if (!orwl_timestart) { + mach_timebase_info_data_t tb = {}; + mach_timebase_info(&tb); + orwl_timebase = tb.numer; + orwl_timebase /= tb.denom; + orwl_timestart = mach_absolute_time(); + } + double diff = (mach_absolute_time() - orwl_timestart) * orwl_timebase; + ts.tv_sec = diff * ORWL_NANO; + ts.tv_nsec = diff - (ts.tv_sec * ORWL_GIGA); return ts_to_abstime(&ts); } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index fd5863727..7b178f7d2 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -271,7 +271,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %lu\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + printf(" %-10s %lu\n", taskmap[idx].name.c_str(), (unsigned long)taskmap[idx].pid); count++; } } -- cgit v1.2.3