mirror of https://github.com/ArduPilot/ardupilot
HAL_QURT: pass argc and argv into main thread
This commit is contained in:
parent
bd2f548130
commit
ea1e3f4928
|
@ -18,15 +18,34 @@ volatile int _last_dsp_line = __LINE__;
|
||||||
volatile const char *_last_dsp_file = __FILE__;
|
volatile const char *_last_dsp_file = __FILE__;
|
||||||
volatile uint32_t _last_counter;
|
volatile uint32_t _last_counter;
|
||||||
|
|
||||||
static void *main_thread(void *)
|
static void *main_thread(void *cmdptr)
|
||||||
{
|
{
|
||||||
|
char *cmdline = (char *)cmdptr;
|
||||||
HAP_PRINTF("Ardupilot main_thread started");
|
HAP_PRINTF("Ardupilot main_thread started");
|
||||||
ArduPilot_main(0, NULL);
|
|
||||||
|
// break cmdline into argc/argv
|
||||||
|
int argc = 0;
|
||||||
|
for (int i=0; cmdline[i]; i++) {
|
||||||
|
if (cmdline[i] == '\n') argc++;
|
||||||
|
}
|
||||||
|
const char **argv = (const char **)calloc(argc+2, sizeof(char *));
|
||||||
|
argv[0] = &cmdline[0];
|
||||||
|
argc = 0;
|
||||||
|
for (int i=0; cmdline[i]; i++) {
|
||||||
|
if (cmdline[i] == '\n') {
|
||||||
|
cmdline[i] = 0;
|
||||||
|
argv[argc+1] = &cmdline[i+1];
|
||||||
|
argc++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
argv[argc] = nullptr;
|
||||||
|
|
||||||
|
ArduPilot_main(argc, argv);
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
uint32_t ardupilot_start()
|
uint32_t ardupilot_start(const char *cmdline, int len)
|
||||||
{
|
{
|
||||||
HAP_PRINTF("Starting Ardupilot");
|
HAP_PRINTF("Starting Ardupilot");
|
||||||
pthread_attr_t thread_attr;
|
pthread_attr_t thread_attr;
|
||||||
|
@ -39,7 +58,7 @@ uint32_t ardupilot_start()
|
||||||
param.sched_priority = APM_MAIN_PRIORITY;
|
param.sched_priority = APM_MAIN_PRIORITY;
|
||||||
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
||||||
|
|
||||||
pthread_create(&thread_ctx, &thread_attr, main_thread, NULL);
|
pthread_create(&thread_ctx, &thread_attr, main_thread, (void *)strdup((char*)cmdline));
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -108,6 +108,27 @@ static void socket_check(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
encode argv/argv as a sequence separated by \n
|
||||||
|
*/
|
||||||
|
static char *encode_argv(int argc, const char *argv[])
|
||||||
|
{
|
||||||
|
uint32_t len = 0;
|
||||||
|
for (int i=0; i<argc; i++) {
|
||||||
|
len += strlen(argv[i]) + 1;
|
||||||
|
}
|
||||||
|
char *ret = (char *)malloc(len+1);
|
||||||
|
char *p = ret;
|
||||||
|
for (int i=0; i<argc; i++) {
|
||||||
|
size_t slen = strlen(argv[i]);
|
||||||
|
strcpy(p, argv[i]);
|
||||||
|
p[slen] = '\n';
|
||||||
|
p += slen + 1;
|
||||||
|
}
|
||||||
|
*p = 0;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
main program
|
main program
|
||||||
*/
|
*/
|
||||||
|
@ -117,8 +138,10 @@ int main(int argc, const char *argv[])
|
||||||
|
|
||||||
printf("Starting DSP code\n");
|
printf("Starting DSP code\n");
|
||||||
send_storage();
|
send_storage();
|
||||||
|
|
||||||
ardupilot_start();
|
char *cmdline = encode_argv(argc, argv);
|
||||||
|
ardupilot_start(cmdline, strlen(cmdline));
|
||||||
|
free(cmdline);
|
||||||
while (true) {
|
while (true) {
|
||||||
uint64_t now = micros64();
|
uint64_t now = micros64();
|
||||||
if (now - last_get_storage_us > 1000*1000) {
|
if (now - last_get_storage_us > 1000*1000) {
|
||||||
|
|
|
@ -2,7 +2,7 @@
|
||||||
|
|
||||||
interface ardupilot {
|
interface ardupilot {
|
||||||
// start main thread
|
// start main thread
|
||||||
uint32 start();
|
uint32 start(in sequence<char> cmdline);
|
||||||
|
|
||||||
// a heartbeat for debugging
|
// a heartbeat for debugging
|
||||||
uint32 heartbeat();
|
uint32 heartbeat();
|
||||||
|
|
Loading…
Reference in New Issue