From 567935dedb1926776f88bbc259cadc1d4464a596 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:35:05 +0200 Subject: [PATCH] QuRT: Fix formatting --- src/platforms/qurt/dspal/dspal_stub.c | 12 +- src/platforms/qurt/include/crc32.h | 2 +- src/platforms/qurt/include/i2c.h | 11 +- src/platforms/qurt/include/queue.h | 32 ++-- src/platforms/qurt/include/sched.h | 3 +- src/platforms/qurt/px4_layer/commands_hil.c | 4 +- src/platforms/qurt/px4_layer/commands_muorb.c | 70 ++++---- src/platforms/qurt/px4_layer/drv_hrt.c | 20 ++- src/platforms/qurt/px4_layer/lib_crc32.c | 85 +++++----- src/platforms/qurt/px4_layer/main.cpp | 114 +++++++------ src/platforms/qurt/px4_layer/params.c | 2 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 24 +-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 153 ++++++++++-------- src/platforms/qurt/px4_layer/qurt_stubs.c | 70 ++++---- src/platforms/qurt/stubs/stubs_posix.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 5 +- .../qurt/tests/hello/hello_example.h | 3 +- .../qurt/tests/hello/hello_start_qurt.cpp | 10 +- .../qurt/tests/muorb/muorb_test_example.h | 3 +- .../qurt/tests/muorb/muorb_test_main.cpp | 2 +- .../tests/muorb/muorb_test_start_qurt.cpp | 14 +- 21 files changed, 344 insertions(+), 297 deletions(-) diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index fc736a169c..13542cdfd3 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -43,25 +43,29 @@ static void do_dlopen() char *error; void (*entry_function)() = NULL; - handle = dlopen ("libdspal_client.so", RTLD_LAZY); + handle = dlopen("libdspal_client.so", RTLD_LAZY); + if (!handle) { printf("Error opening libdspal_client.so\n"); return 1; } + entry_function = dlsym(handle, "dspal_entry"); + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { printf("Error dlsym for dspal_entry"); ret = 2; } + dlclose(handle); #endif } - -int main(int argc, char* argv[]) + +int main(int argc, char *argv[]) { int ret = 0; - char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"}; + char *builtin[] = {"libgcc.so", "libc.so", "libstdc++.so"}; printf("In DSPAL main\n"); dlinit(3, builtin); diff --git a/src/platforms/qurt/include/crc32.h b/src/platforms/qurt/include/crc32.h index bf828e3e0e..34e080b1c2 100644 --- a/src/platforms/qurt/include/crc32.h +++ b/src/platforms/qurt/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h index ee3e4196dd..5021cf1814 100644 --- a/src/platforms/qurt/include/i2c.h +++ b/src/platforms/qurt/include/i2c.h @@ -34,10 +34,9 @@ ****************************************************************************/ #pragma once -struct i2c_msg_s -{ - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; +struct i2c_msg_s { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; }; diff --git a/src/platforms/qurt/include/queue.h b/src/platforms/qurt/include/queue.h index 4d95541e02..30dc264f3f 100644 --- a/src/platforms/qurt/include/queue.h +++ b/src/platforms/qurt/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h index 7d7724c5fa..b67db23b1d 100644 --- a/src/platforms/qurt/include/sched.h +++ b/src/platforms/qurt/include/sched.h @@ -3,8 +3,7 @@ #define SCHED_FIFO 1 #define SCHED_RR 2 -struct sched_param -{ +struct sched_param { int sched_priority; }; diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index 3ff299b89d..87aa6479f8 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -41,7 +41,7 @@ const char *get_commands(void); const char *get_commands() { - static const char *commands = + static const char *commands = "uorb start\n" "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" @@ -104,5 +104,5 @@ const char *get_commands() ; return commands; - + } diff --git a/src/platforms/qurt/px4_layer/commands_muorb.c b/src/platforms/qurt/px4_layer/commands_muorb.c index 27318af9c6..de6381a35a 100644 --- a/src/platforms/qurt/px4_layer/commands_muorb.c +++ b/src/platforms/qurt/px4_layer/commands_muorb.c @@ -39,42 +39,42 @@ const char *get_commands() { - static const char *commands = - "uorb start\n" - "muorb_test start\n"; + static const char *commands = + "uorb start\n" + "muorb_test start\n"; -/* - "hil mode_pwm\n" - "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; -*/ -/* - "param show\n" - "param set CAL_GYRO_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" - "gyrosim start\n" - "accelsim start\n" - "rgbled start\n" - "tone_alarm start\n" - "simulator start -s\n" - "commander start\n" - "sensors start\n" - "ekf_att_pos_estimator start\n" - "mc_pos_control start\n" - "mc_att_control start\n" - "param set MAV_TYPE 2\n" - "param set RC1_MAX 2015\n" - "param set RC1_MIN 996\n" - "param set RC_TRIM 1502\n" -*/ + /* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; + */ + /* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" + */ return commands; -/*====================================== Working set -======================================*/ - - //"muorb_test start\n" - //"gyrosim start\n" - //"adcsim start\n" - + /*====================================== Working set + ======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index b5d6bd20ff..c15c818172 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -219,7 +219,7 @@ hrt_call_enter(struct hrt_call *entry) * * This routine simulates a timer interrupt handler */ -static void +static void hrt_tim_isr(void *p) { @@ -249,7 +249,7 @@ hrt_call_reschedule() uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); - + /* * Determine what the next deadline will be. * @@ -275,10 +275,10 @@ hrt_call_reschedule() } } - // There is no timer ISR, so simulate one by putting an event on the + // There is no timer ISR, so simulate one by putting an event on the // high priority work queue //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void @@ -286,6 +286,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte { //printf("hrt_call_internal\n"); hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised @@ -295,8 +296,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -360,17 +362,20 @@ hrt_call_invoke(void) hrt_abstime deadline; hrt_lock(); + while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); @@ -404,6 +409,7 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c index 4ba6fbf6df..c14ebfceeb 100644 --- a/src/platforms/qurt/px4_layer/lib_crc32.c +++ b/src/platforms/qurt/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 15907ded78..fc8801b60e 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,8 +50,8 @@ using namespace std; -extern void init_app_map(map &apps); -extern void list_builtins(map &apps); +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); static px4_task_t g_dspal_task = -1; __BEGIN_DECLS @@ -61,24 +61,27 @@ extern const char *get_commands(void); void qurt_external_hook(void) __attribute__((weak)); __END_DECLS -static void run_cmd(map &apps, const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) +{ // command is appargs[0] string command = appargs[0]; + if (apps.find(command) != apps.end()) { - const char *arg[2+1]; + const char *arg[2 + 1]; unsigned int i = 0; + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } + arg[i] = (char *)0; //PX4_DEBUG_PRINTF(i); - apps[command](i,(char **)arg); - } - else - { + apps[command](i, (char **)arg); + + } else { PX4_WARN("NOT FOUND."); list_builtins(apps); } @@ -88,14 +91,15 @@ void eat_whitespace(const char *&b, int &i) { // Eat whitespace while (b[i] == ' ' || b[i] == '\t') { ++i; } + b = &b[i]; - i=0; + i = 0; } -static void process_commands(map &apps, const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; - int i=0; + int i = 0; const char *b = cmds; bool found_first_char = false; char arg[256]; @@ -103,7 +107,7 @@ static void process_commands(map &apps, const char *cmds) // Eat leading whitespace eat_whitespace(b, i); - for(;;) { + for (;;) { // End of command line if (b[i] == '\n' || b[i] == '\0') { strncpy(arg, b, i); @@ -112,20 +116,26 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { - PX4_WARN("Processing command: %s",appargs[0].c_str()); - for(int ai=1;ai<(int)appargs.size();ai++) - PX4_WARN(" > arg: %s",appargs[ai].c_str()); - run_cmd(apps, appargs); + PX4_WARN("Processing command: %s", appargs[0].c_str()); + + for (int ai = 1; ai < (int)appargs.size(); ai++) { + PX4_WARN(" > arg: %s", appargs[ai].c_str()); + } + + run_cmd(apps, appargs); } + appargs.clear(); + if (b[i] == '\n') { eat_whitespace(b, ++i); continue; - } - else { + + } else { break; } } + // End of arg else if (b[i] == ' ') { strncpy(arg, b, i); @@ -134,11 +144,13 @@ static void process_commands(map &apps, const char *cmds) eat_whitespace(b, ++i); continue; } + ++i; } } -namespace px4 { +namespace px4 +{ extern void init_once(void); }; @@ -147,23 +159,25 @@ int dspal_main(int argc, char *argv[]); __END_DECLS -int dspal_entry( int argc, char* argv[] ) +int dspal_entry(int argc, char *argv[]) { PX4_INFO("In main\n"); - map apps; + map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); sleep(1); // give time for all commands to execute before starting external function - if(qurt_external_hook) - { + + if (qurt_external_hook) { qurt_external_hook(); } - for(;;){ + + for (;;) { sleep(1); } - return 0; + + return 0; } static void usage() @@ -174,32 +188,32 @@ static void usage() extern "C" { -int dspal_main(int argc, char *argv[]) -{ - int ret = 0; - if (argc == 2 && strcmp(argv[1], "start") == 0) { + int dspal_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 2 && strcmp(argv[1], "start") == 0) { g_dspal_task = px4_task_spawn_cmd("dspal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - dspal_entry, - argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); - } - else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_dspal_task < 0) { - PX4_WARN("start up thread not running"); - } - else { - px4_task_delete(g_dspal_task); - g_dspal_task = -1; - } - } - else { - usage(); - ret = -1; - } + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); - return ret; -} + } else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + + } else { + usage(); + ret = -1; + } + + return ret; + } } diff --git a/src/platforms/qurt/px4_layer/params.c b/src/platforms/qurt/px4_layer/params.c index 3ad35a6a12..654b07df00 100644 --- a/src/platforms/qurt/px4_layer/params.c +++ b/src/platforms/qurt/px4_layer/params.c @@ -39,5 +39,5 @@ /** * @board QuRT_App */ -PARAM_DEFINE_INT32(MAV_TYPE,2); +PARAM_DEFINE_INT32(MAV_TYPE, 2); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 14ef3304bc..be419d4069 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -61,11 +61,13 @@ extern uint64_t get_ticks_per_us(); //long PX4_TICKS_PER_SEC = 1000L; -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i=0; i< sec; i++) +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i = 0; i < sec; i++) { usleep(1000000); - return 0; + } + + return 0; } extern void hrt_init(void); @@ -96,16 +98,16 @@ void init_once(void) { // Required for QuRT //_posix_init(); - PX4_WARN( "Before calling work_queue_init" ); + PX4_WARN("Before calling work_queue_init"); // _shell_task_id = pthread_self(); // PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); - PX4_WARN( "Before calling hrt_init" ); + PX4_WARN("Before calling hrt_init"); hrt_work_queue_init(); hrt_init(); - PX4_WARN( "after calling hrt_init" ); + PX4_WARN("after calling hrt_init"); } void init(int argc, char *argv[], const char *app_name) @@ -142,9 +144,11 @@ dm_write( size_t strnlen(const char *s, size_t maxlen) { - size_t i=0; - while (s[i] != '\0' && i < maxlen) + size_t i = 0; + + while (s[i] != '\0' && i < maxlen) { ++i; + } return i; } @@ -154,7 +158,7 @@ int ioctl(int a, int b, unsigned long c) return -1; } -int write(int a, char const* b, int c) +int write(int a, char const *b, int c) { return -1; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 0e7fdc209a..4216bbf599 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -66,8 +66,7 @@ pthread_t _shell_task_id = 0; -struct task_entry -{ +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -76,31 +75,30 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); - PX4_WARN( "Before waiting infinte busy loop" ); - //for( ;; ) - //{ - // volatile int x = 0; - // ++x; - // } + PX4_WARN("Before waiting infinte busy loop"); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); - px4_task_exit(0); + px4_task_exit(0); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -108,7 +106,8 @@ px4_systemreset(bool to_bootloader) PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -116,83 +115,96 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; PX4_DEBUG("Creating %s\n", name); - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; - } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); - pthdata_t *taskdata; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; - taskdata->entry = entry; + if (p == (char *)0) { + break; + } + + ++argc; + len += strlen(p) + 1; + } + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; + + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; } + #endif - size_t fixed_stacksize = -1; - pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size ); - fixed_stacksize = 8 * 1024; - fixed_stacksize = ( fixed_stacksize < (size_t)stack_size )? (size_t)stack_size:fixed_stacksize; + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); + fixed_stacksize = 8 * 1024; + fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; + + PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); - PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize ); - pthread_attr_setstacksize(&attr, fixed_stacksize); - //pthread_attr_setstacksize(&attr, stack_size); - param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { return (rv < 0) ? rv : -rv; } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -212,15 +226,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_WARN("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -232,20 +249,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -258,10 +276,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -275,15 +295,17 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } @@ -297,11 +319,12 @@ int px4_getpid() // return SHELL_TASK_ID; // Get pthread ID from the opaque ID - for (int i=0; i -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index e4180307ce..56aa52b478 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -71,11 +71,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 266b0a2b42..68236eb854 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -37,7 +37,8 @@ #include "uORB/topics/esc_status.h" #include "uORB/topics/vehicle_command.h" -class MuorbTestExample { +class MuorbTestExample +{ public: MuorbTestExample() {}; ~MuorbTestExample() {}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 8f8b7640e2..c6818a9270 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -42,7 +42,7 @@ #include #include "muorb_test_example.h" -extern "C" __EXPORT int muorb_test_entry( int argc, char** argv ); +extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); int muorb_test_entry(int argc, char **argv) { diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index 6d64cfec8b..ba44169ec3 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -80,15 +80,15 @@ int muorb_test_main(int argc, char *argv[]) /* this is not an error */ return 0; } - - PX4_INFO( "before starting the muorb_test_entry task" ); + + PX4_INFO("before starting the muorb_test_entry task"); daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - muorb_test_entry, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 8192, + muorb_test_entry, + (char *const *)argv); return 0; }