fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 22:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

QuRT: Fix formatting

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:35:05 +02:00
parent 73545a4311
commit 567935dedb
21 changed files with 344 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/qurt/dspal/dspal_stub.c
+8 -4
View File
@@ -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);
src/platforms/qurt/include/crc32.h
+1 -1
View File
@@ -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
src/platforms/qurt/include/i2c.h
+5 -6
View File
@@ -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;
};
src/platforms/qurt/include/queue.h
+14 -18
View File
@@ -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);
src/platforms/qurt/include/sched.h
+1 -2
View File
@@ -3,8 +3,7 @@
#define SCHED_FIFO 1
#define SCHED_RR 2
struct sched_param
{
struct sched_param {
int sched_priority;
};
src/platforms/qurt/px4_layer/commands_hil.c
+2 -2
View File
@@ -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;
}
src/platforms/qurt/px4_layer/commands_muorb.c
+35 -35
View File
@@ -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"
}
src/platforms/qurt/px4_layer/drv_hrt.c
+13 -7
View File
@@ -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();
}
src/platforms/qurt/px4_layer/lib_crc32.c
+42 -43
View File
@@ -48,46 +48,45 @@
#include <crc32.h>
// 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);
}
src/platforms/qurt/px4_layer/main.cpp
+64 -50
View File
@@ -50,8 +50,8 @@
using namespace std;
extern void init_app_map(map<string,px4_main_t> &apps);
extern void list_builtins(map<string,px4_main_t> &apps);
extern void init_app_map(map<string, px4_main_t> &apps);
extern void list_builtins(map<string, px4_main_t> &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<string,px4_main_t> &apps, const vector<string> &appargs) {
static void run_cmd(map<string, px4_main_t> &apps, const vector<string> &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<string,px4_main_t> &apps, const char *cmds)
static void process_commands(map<string, px4_main_t> &apps, const char *cmds)
{
vector<string> 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<string,px4_main_t> &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<string,px4_main_t> &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<string,px4_main_t> &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<string,px4_main_t> apps;
map<string, px4_main_t> 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;
}
}
src/platforms/qurt/px4_layer/params.c
+1 -1
View File
@@ -39,5 +39,5 @@
/**
* @board QuRT_App
*/
PARAM_DEFINE_INT32(MAV_TYPE,2);
PARAM_DEFINE_INT32(MAV_TYPE, 2);
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+14 -10
View File
@@ -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;
}
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+88 -65
View File
@@ -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; i<argc; i++) {
for (i = 0; i < argc; i++) {
PX4_DEBUG("arg %d %s\n", i, argv[i]);
taskdata->argv[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, &param);
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; ++i) {
for (i = 0; i < PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused == false) {
taskmap[i].pid = task;
taskmap[i].name = name;
@@ -200,10 +212,12 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
break;
}
}
if (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; ++i) {
for (i = 0; i < PX4_MAX_TASKS; ++i) {
if (taskmap[i].pid == pid) {
taskmap[i].isused = false;
break;
}
}
if (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<PX4_MAX_TASKS; ++i) {
for (int i = 0; i < PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused && taskmap[i].pid == pid) {
return i;
}
}
PX4_ERR("px4_getpid() called from unknown thread context!");
return -EINVAL;
}
@@ -310,14 +333,14 @@ int px4_getpid()
const char *getprogname();
const char *getprogname()
{
pthread_t pid = pthread_self();
for (int i=0; i<PX4_MAX_TASKS; i++)
{
if (taskmap[i].isused && taskmap[i].pid == pid)
{
pthread_t pid = pthread_self();
for (int i = 0; i < PX4_MAX_TASKS; i++) {
if (taskmap[i].isused && taskmap[i].pid == pid) {
return taskmap[i].name.c_str();
}
}
return "Unknown App";
}
__END_DECLS
src/platforms/qurt/px4_layer/qurt_stubs.c
+35 -35
View File
@@ -46,75 +46,75 @@ void _Read_sleb(void);
void __cxa_guard_acquire(void);
void __cxa_pure_virtual(void);
void block_indefinite( void )
void block_indefinite(void)
{
sem_t forever;
sem_init(&forever, 0, 0);
sem_wait(&forever);
}
void _Read_uleb( void )
void _Read_uleb(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Parse_fde_instr( void )
void _Parse_fde_instr(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Parse_csd( void )
void _Parse_csd(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Valbytes( void )
void _Valbytes(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Get_eh_data( void )
void _Get_eh_data(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Parse_lsda( void )
void _Parse_lsda(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void __cxa_guard_release( void )
void __cxa_guard_release(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Read_enc_ptr( void )
void _Read_enc_ptr(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void _Read_sleb( void )
void _Read_sleb(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void __cxa_guard_acquire( void )
void __cxa_guard_acquire(void)
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
void __cxa_pure_virtual()
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
void __cxa_pure_virtual()
{
PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__);
block_indefinite();
}
src/platforms/qurt/stubs/stubs_posix.c
+1 -1
View File
@@ -79,7 +79,7 @@ int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *p
return 1;
}
int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine) (void *), void *arg)
int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg)
{
return 1;
}
src/platforms/qurt/tests/hello/hello_example.cpp
+3 -2
View File
@@ -49,8 +49,9 @@ int HelloExample::main()
{
appState.setRunning(true);
int i=0;
while (!appState.exitRequested() && i<5) {
int i = 0;
while (!appState.exitRequested() && i < 5) {
PX4_DEBUG(" Doing work...");
++i;
src/platforms/qurt/tests/hello/hello_example.h
+2 -1
View File
@@ -41,7 +41,8 @@
#include <px4_app.h>
class HelloExample {
class HelloExample
{
public:
HelloExample() {};
src/platforms/qurt/tests/hello/hello_start_qurt.cpp
+5 -5
View File
@@ -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;
}
src/platforms/qurt/tests/muorb/muorb_test_example.h
+2 -1
View File
@@ -37,7 +37,8 @@
#include "uORB/topics/esc_status.h"
#include "uORB/topics/vehicle_command.h"
class MuorbTestExample {
class MuorbTestExample
{
public:
MuorbTestExample() {};
~MuorbTestExample() {};
src/platforms/qurt/tests/muorb/muorb_test_main.cpp
+1 -1
View File
@@ -42,7 +42,7 @@
#include <px4_app.h>
#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)
{
src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp
+7 -7
View File
@@ -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;
}