Skip to content

Commit

Permalink
Solved C++ linking mystery with lots of hackerygit checkout -b eric-l…
Browse files Browse the repository at this point in the history
…ink-fix-messy!
  • Loading branch information
katzfey committed May 21, 2024
1 parent ec9d4c1 commit fe6a231
Show file tree
Hide file tree
Showing 8 changed files with 167 additions and 32 deletions.
33 changes: 33 additions & 0 deletions ArduCopter/Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -840,3 +840,36 @@ Copter copter;
AP_Vehicle& vehicle = copter;

AP_HAL_MAIN_CALLBACKS(&copter);

#include <AP_HAL_QURT/interface.h>

class ValueTest {
public:
ValueTest(int val) : _value(val) {};
~ValueTest() {};

int getValue() const { return _value; }

private:
int _value;
};

const ValueTest& get_ValueTest(int init) {
static const ValueTest *vt;
if (vt == nullptr) {
vt = new ValueTest(init);
}
return *vt;
}

const ValueTest& myValue = get_ValueTest(5);

int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
{
HAP_PRINTF("The address is %p", &myValue);
if (&myValue != nullptr) {
HAP_PRINTF("The value is %d", myValue.getValue());
}

return 0;
}
36 changes: 26 additions & 10 deletions Tools/ardupilotwaf/boards.py
Original file line number Diff line number Diff line change
Expand Up @@ -1666,8 +1666,7 @@ def configure_env(self, cfg, env):
super(QURT, self).configure_env(cfg, env)

env.BOARD_CLASS = "QURT"
env.HEAXGON_APP = "libardupilot.so"
env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -fpic -shared -call_shared -mG0lib -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o"
env.HEXAGON_APP = "libardupilot.so"
env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"]
env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/posix"]
env.CXXFLAGS += ["-fPIC", "-MD"]
Expand Down Expand Up @@ -1696,16 +1695,33 @@ def configure_env(self, cfg, env):
"--wrap=memalign",
"--wrap=__stack_chk_fail",
"-lc",
f"-soname={cfg.env.HEXAGON_APP}",
"--start-group",
"--whole-archive",
"--end-group",
"--start-group",
"-lgcc",
"--end-group",
cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/finiS.o"
"-lc++"
# "-lc++",
# "--start-group",
# "--whole-archive",
# "-lgcc",
# "--end-group",
# cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/finiS.o",
]

# diff --git a/waflib/Tools/cxx.py b/waflib/Tools/cxx.py
# index 705ec74d..202afcb2 100644
# --- a/waflib/Tools/cxx.py
# +++ b/waflib/Tools/cxx.py
# @@ -25,7 +25,8 @@ class cxx(Task.Task):
#
# class cxxprogram(link_task):
# "Links object files into c++ programs"
# - run_str = '${LINK_CXX} ${LINKFLAGS} ${CXXLNK_SRC_F}${SRC} ${CXXLNK_TGT_F}${TGT[0].relpath()} ${RPATH_ST:RPATH} ${FRAMEWORKPATH_ST:FRAMEWORKPATH} ${FRAMEWORK_ST:FRAMEWORK} ${ARCH_ST:ARCH} ${STLIB_MARKER} ${STLIBPATH_ST:STLIBPATH} ${STLIB_ST:STLIB} ${SHLIB_MARKER} ${LIBPATH_ST:LIBPATH} ${LIB_ST:LIB} ${LDFLAGS}'
# + # run_str = '${LINK_CXX} ${LINKFLAGS} ${CXXLNK_SRC_F}${SRC} ${CXXLNK_TGT_F}${TGT[0].relpath()} ${RPATH_ST:RPATH} ${FRAMEWORKPATH_ST:FRAMEWORKPATH} ${FRAMEWORK_ST:FRAMEWORK} ${ARCH_ST:ARCH} ${STLIB_MARKER} ${STLIBPATH_ST:STLIBPATH} ${STLIB_ST:STLIB} ${SHLIB_MARKER} ${LIBPATH_ST:LIBPATH} ${LIB_ST:LIB} ${LDFLAGS}'
# + run_str = '${LINK_CXX} ${CXXLNK_TGT_F} ${TGT[0].relpath()} ${LINKFLAGS} ${STLIBPATH_ST:STLIBPATH} --start-group --whole-archive ${CXXLNK_SRC_F}${SRC} ${STLIB_ST:STLIB} --end-group --start-group -lgcc --end-group /opt/hexagon-sdk/4.1.0.4-lite/tools/HEXAGON_Tools/8.4.05/Tools/target/hexagon/lib/v66/G0/pic/finiS.o'
# vars = ['LINKDEPS']
# ext_out = ['.bin']
# inst_to = '${BINDIR}'


# cfg.env.LDPATH = "--end-group"

if not cfg.env.DEBUG:
env.CXXFLAGS += [
'-O2',
Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_HAL/AP_HAL_Main.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,9 @@
#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, CALLBACKS); \
HAP_PRINTF("About to call hal.run %p", &hal); \
/* hal.run(argc, argv, CALLBACKS); */ \
AP_HAL::get_HAL().run(argc, argv, CALLBACKS); \
return 0; \
} \
}
2 changes: 2 additions & 0 deletions libraries/AP_HAL/board/qurt.h
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#pragma once

#include <AP_HAL_QURT/AP_HAL_QURT_Main.h>

#define HAL_BOARD_NAME "QURT"
#define HAL_CPU_CLASS HAL_CPU_CLASS_1000
#define HAL_MEM_CLASS HAL_MEM_CLASS_1000
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h
Original file line number Diff line number Diff line change
Expand Up @@ -13,3 +13,9 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once

#ifdef AP_MAIN
#undef AP_MAIN
#endif

#define AP_MAIN qurt_arducopter_main
24 changes: 15 additions & 9 deletions libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,25 +77,31 @@ HAL_QURT::HAL_QURT() :

void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
{
assert(callbacks);
HAP_PRINTF("In HAL_QURT::run");

assert(callbacks);

/* initialize all drivers and private members here.
* up to the programmer to do this in the correct order.
* Scheduler should likely come first. */
scheduler->init();
HAP_PRINTF("scheduler: %p", scheduler);
HAP_PRINTF("schedulerInstance: %p", &schedulerInstance);
// scheduler->init();
schedulerInstance.init();
schedulerInstance.hal_initialized();
serial0Driver.begin(115200);
rcinDriver.init();
callbacks->setup();
scheduler->set_system_initialized();
// serial0Driver.begin(115200);
// rcinDriver.init();
// callbacks->setup();
// scheduler->set_system_initialized();

for (;;) {
callbacks->loop();
}
// for (;;) {
// callbacks->loop();
// }
}

const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_QURT *hal;
HAP_PRINTF("In HAL_QURT get_HAL. %p", hal);
if (hal == nullptr) {
hal = new HAL_QURT;
HAP_PRINTF("allocated HAL_QURT of size %u", sizeof(*hal));
Expand Down
18 changes: 10 additions & 8 deletions libraries/AP_HAL_QURT/Scheduler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ Scheduler::Scheduler()

void Scheduler::init()
{
HAP_PRINTF("In Scheduler::init");

_main_task_pid = getpid();

// setup the timer thread - this will call tasks at 1kHz
Expand All @@ -36,28 +38,28 @@ void Scheduler::init()

pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);

param.sched_priority = APM_TIMER_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);

pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this);

// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);

param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);

pthread_create(&_uart_thread_ctx, &thread_attr, &Scheduler::_uart_thread, this);

// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);

param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);

pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this);
}

Expand Down
76 changes: 72 additions & 4 deletions libraries/AP_HAL_QURT/replace.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <pthread.h>
#include "replace.h"
#include "interface.h"
#include <AP_HAL/AP_HAL.h>

extern "C" {

Expand Down Expand Up @@ -133,10 +134,46 @@ int ArduPilot_main(int argc, const char *argv[])

}

int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
{
return 0;
}
// class ValueTest {
// public:
// ValueTest(int val) : _value(val) {};
// ~ValueTest() {};
//
// int getValue() const { return _value; }
//
// private:
// int _value;
// };
//
// const ValueTest& get_ValueTest(int init) {
// static const ValueTest *vt;
// if (vt == nullptr) {
// vt = new ValueTest(init);
// }
// return *vt;
// }
//
// const ValueTest& myValue = get_ValueTest(5);
//
// extern "C" int qurt_arducopter_main(int argc, char* const argv[]);
// int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
// {
// HAP_PRINTF("The address is %p", &myValue);
// if (&myValue != nullptr) {
// HAP_PRINTF("The value is %d", myValue.getValue());
// }
//
// // HAP_PRINTF("About to call HAL_QURT::run");
// //
// // const AP_HAL::HAL &hal = AP_HAL::get_HAL();
// // hal.run(0, 0, 0);
//
// // HAP_PRINTF("About to call qurt_arducopter_main %p", &qurt_arducopter_main);
// //
// // qurt_arducopter_main(0, NULL);
//
// return 0;
// }

int px4muorb_topic_advertised(const char *name)
{
Expand All @@ -162,3 +199,34 @@ float px4muorb_get_cpu_load(void)
{
return 0.0f;
}

extern "C" void free(void *ptr) {
return;
}

// malloc
extern "C" void *malloc(size_t size) {
return NULL;
}

// posix_memalign
extern "C" int posix_memalign(void **memptr, size_t alignment, size_t size) {
return 0;
}

// calloc
extern "C" void *calloc(size_t nmemb, size_t size) {
return NULL;
}


// realloc
extern "C" void *realloc(void *ptr, size_t size) {
return NULL;
}

// nanosleep
#include <time.h>
extern "C" int nanosleep(const struct timespec *req, struct timespec *_Nullable rem) {
return 0;
}

0 comments on commit fe6a231

Please sign in to comment.