From e5c7ae470640ea20fe372b8da8185abf72123dbe Mon Sep 17 00:00:00 2001 From: Freddie Chopin Date: Fri, 30 Nov 2012 11:04:35 +0100 Subject: [PATCH 01/10] Make cpuload correct and more efficient for all configurations of NuttX Currently cpuload assumes there are only 2 static threads - idle and init, this is true only for "basic" config of NuttX, as there can be 3 more static threads: paging, work0 and work1 - depending on config. In such cases cpuload would mistake one of them for init (which in fact is always last), giving incorrect results to "top" Signed-off-by: Freddie Chopin --- apps/systemlib/cpuload.c | 40 ++++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c index 20b711fa66..e35bc56c5d 100644 --- a/apps/systemlib/cpuload.c +++ b/apps/systemlib/cpuload.c @@ -77,8 +77,6 @@ extern FAR _TCB *sched_gettcb(pid_t pid); void cpuload_initialize_once() { -// if (!system_load.initialized) -// { system_load.start_time = hrt_absolute_time(); int i; @@ -86,27 +84,29 @@ void cpuload_initialize_once() system_load.tasks[i].valid = false; } - system_load.total_count = 0; - uint64_t now = hrt_absolute_time(); - /* initialize idle thread statically */ - system_load.tasks[0].start_time = now; - system_load.tasks[0].total_runtime = 0; - system_load.tasks[0].curr_start_time = 0; - system_load.tasks[0].tcb = sched_gettcb(0); - system_load.tasks[0].valid = true; - system_load.total_count++; + int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" - /* initialize init thread statically */ - system_load.tasks[1].start_time = now; - system_load.tasks[1].total_runtime = 0; - system_load.tasks[1].curr_start_time = 0; - system_load.tasks[1].tcb = sched_gettcb(1); - system_load.tasks[1].valid = true; - /* count init thread */ - system_load.total_count++; - // } +#ifdef CONFIG_PAGING + static_tasks_count++; // include paging thread in initialization +#endif /* CONFIG_PAGING */ +#if CONFIG_SCHED_WORKQUEUE + static_tasks_count++; // include high priority work0 thread in initialization +#endif /* CONFIG_SCHED_WORKQUEUE */ +#if CONFIG_SCHED_LPWORK + static_tasks_count++; // include low priority work1 thread in initialization +#endif /* CONFIG_SCHED_WORKQUEUE */ + + // perform static initialization of "system" threads + for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) + { + system_load.tasks[system_load.total_count].start_time = now; + system_load.tasks[system_load.total_count].total_runtime = 0; + system_load.tasks[system_load.total_count].curr_start_time = 0; + system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs + system_load.tasks[system_load.total_count].valid = true; + } } void sched_note_start(FAR _TCB *tcb) From 5b93ab0372dd1208112156850908b87143a0c0dd Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 18 Mar 2013 22:36:47 +0100 Subject: [PATCH 02/10] Clean up and compact the output to fit inside a 80 column display. Bug fix: - running/sleeping count Plus: - added task state - show the idle task (to make the number of tasks match the reported number) - convert some calc to floating point where it doesn't hurt performance (for clarity) - accept 'q' (standard) and escape to exit the program --- apps/systemcmds/top/top.c | 267 +++++++++++++++++++++----------------- 1 file changed, 148 insertions(+), 119 deletions(-) diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index 59d2bc8f16..d49cf2f5b5 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -49,19 +49,46 @@ #include #include +#define CL "\033[K" // clear line + /** * Start the top application. */ -__EXPORT int top_main(int argc, char *argv[]); +__EXPORT int top_main(void); extern struct system_load_s system_load; -bool top_sigusr1_rcvd = false; - -int top_main(int argc, char *argv[]) +static const char * +tstate_name(const tstate_t s) { - int t; + switch (s) { + case TSTATE_TASK_INVALID: return "init"; + case TSTATE_TASK_PENDING: return "PEND"; + case TSTATE_TASK_READYTORUN: return "READY"; + case TSTATE_TASK_RUNNING: return "RUN"; + + case TSTATE_TASK_INACTIVE: return "inact"; + case TSTATE_WAIT_SEM: return "w:sem"; +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: return "w:sig"; +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe"; + case TSTATE_WAIT_MQNOTFULL: return "w:mqf"; +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: return "w:pgf"; +#endif + + default: + return "ERROR"; + } +} + +int +top_main(void) +{ uint64_t total_user_time = 0; int running_count = 0; @@ -73,7 +100,7 @@ int top_main(int argc, char *argv[]) uint64_t last_times[CONFIG_MAX_TASKS]; float curr_loads[CONFIG_MAX_TASKS]; - for (t = 0; t < CONFIG_MAX_TASKS; t++) + for (int t = 0; t < CONFIG_MAX_TASKS; t++) last_times[t] = 0; float interval_time_ms_inv = 0.f; @@ -81,16 +108,16 @@ int top_main(int argc, char *argv[]) /* Open console directly to grab CTRL-C signal */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - while (true) -// for (t = 0; t < 10; t++) - { - int i; + /* clear screen */ + printf("\033[2J"); - uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU); - unsigned int curr_time_s = curr_time_ms / 1000LLU; + for (;;) { + int i; + uint64_t curr_time_us; + uint64_t idle_time_us; - uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU); - unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU; + curr_time_us = hrt_absolute_time(); + idle_time_us = system_load.tasks[0].total_runtime; if (new_time > interval_start_time) interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); @@ -100,7 +127,38 @@ int top_main(int argc, char *argv[]) total_user_time = 0; for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0; + uint64_t interval_runtime; + + if (system_load.tasks[i].valid) { + switch (system_load.tasks[i].tcb->task_state) { + case TSTATE_TASK_PENDING: + case TSTATE_TASK_READYTORUN: + case TSTATE_TASK_RUNNING: + running_count++; + break; + + case TSTATE_TASK_INVALID: + case TSTATE_TASK_INACTIVE: + case TSTATE_WAIT_SEM: +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: + case TSTATE_WAIT_MQNOTFULL: +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: +#endif + blocked_count++; + break; + } + } + + interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && + system_load.tasks[i].total_runtime > last_times[i]) + ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 + : 0; last_times[i] = system_load.tasks[i].total_runtime; @@ -109,7 +167,6 @@ int top_main(int argc, char *argv[]) if (i > 0) total_user_time += interval_runtime; - } else curr_loads[i] = 0; } @@ -117,127 +174,99 @@ int top_main(int argc, char *argv[]) for (i = 0; i < CONFIG_MAX_TASKS; i++) { if (system_load.tasks[i].valid && (new_time > interval_start_time)) { if (system_load.tasks[i].tcb->pid == 0) { - float idle = curr_loads[0]; - float task_load = (float)(total_user_time) * interval_time_ms_inv; + float idle; + float task_load; + float sched_load; - if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ + idle = curr_loads[0]; + task_load = (float)(total_user_time) * interval_time_ms_inv; - float sched_load = 1.f - idle - task_load; + /* this can happen if one tasks total runtime was not computed + correctly by the scheduler instrumentation TODO */ + if (task_load > (1.f - idle)) + task_load = (1.f - idle); + + sched_load = 1.f - idle - task_load; /* print system information */ - printf("\033[H"); /* cursor home */ - printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count); - printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100)); - printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000)); + printf("\033[H"); /* move cursor home and clear screen */ + printf(CL "Processes: %d total, %d running, %d sleeping\n", + system_load.total_count, + running_count, + blocked_count); + printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); + printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d); - /* 34 chars command name length (32 chars plus two spaces) */ - char header_spaces[CONFIG_TASK_NAME_SIZE + 1]; - memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); - header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; + /* header for task list */ + printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", #if CONFIG_RR_INTERVAL > 0 - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); + "TSLICE" #else - printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces); -#endif - - } else { - enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state; - - if (task_state == TSTATE_TASK_PENDING || - task_state == TSTATE_TASK_READYTORUN || - task_state == TSTATE_TASK_RUNNING) { - running_count++; - } - - if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */ - task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */ -#ifndef CONFIG_DISABLE_SIGNALS - || task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */ -#endif -#ifndef CONFIG_DISABLE_MQUEUE - || task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */ - || task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */ -#endif -#ifdef CONFIG_PAGING - || task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */ -#endif - ) { - blocked_count++; - } - - char spaces[CONFIG_TASK_NAME_SIZE + 2]; - - /* count name len */ - int namelen = 0; - - while (namelen < CONFIG_TASK_NAME_SIZE) { - if (system_load.tasks[i].tcb->name[namelen] == '\0') break; - - namelen++; - } - - int s = 0; - - for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) { - spaces[s] = ' '; - } - - spaces[s] = '\0'; - - char *runtime_spaces = " "; - - if ((system_load.tasks[i].total_runtime / 1000) < 99) { - runtime_spaces = ""; - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) - break; - - stack_free++; - } - - printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u", - (int)system_load.tasks[i].tcb->pid, - system_load.tasks[i].tcb->name, - spaces, - (system_load.tasks[i].total_runtime / 1000), - runtime_spaces, - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), - stack_size - stack_free, - stack_size); - /* Print scheduling info with RR time slice */ -#if CONFIG_RR_INTERVAL > 0 - printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); -#else - /* Print scheduling info without time slice*/ - printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority); + "STATE" #endif + ); } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) + break; + + stack_free++; + } + + printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(curr_loads[i] * 100), + (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); + +#if CONFIG_RR_INTERVAL > 0 + /* print scheduling info with RR time slice */ + printf(" %6d\n", system_load.tasks[i].tcb->timeslice); +#else + // print task state instead + printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); +#endif } } - printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J"); - fflush(stdout); - interval_start_time = new_time; - char c; - - /* Sleep 200 ms waiting for user input four times */ + /* Sleep 200 ms waiting for user input five times ~ 1s */ /* XXX use poll ... */ - for (int k = 0; k < 4; k++) { + for (int k = 0; k < 5; k++) { + char c; + if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - printf("Abort\n"); + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': close(console); return OK; + /* not reached */ } } From 496127ca459c603e5de3f8bc83c6113bcf9cbead Mon Sep 17 00:00:00 2001 From: sergeil Date: Fri, 31 May 2013 11:44:20 +0200 Subject: [PATCH 03/10] mpu6000 driver support for setting rate --- src/drivers/mpu6000/mpu6000.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index df1958186f..2208425369 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -272,6 +272,11 @@ private: */ void _set_dlpf_filter(uint16_t frequency_hz); + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(uint16_t desired_sample_rate_hz); + }; /** @@ -378,7 +383,8 @@ MPU6000::init() up_udelay(1000); // SAMPLE RATE - write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + _set_sample_rate(200); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) @@ -493,6 +499,18 @@ MPU6000::probe() return -EIO; } +/* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro +*/ +void +MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +{ + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_reg(MPUREG_SMPLRT_DIV, div-1); +} + /* set the DLPF filter frequency. This affects both accel and gyro. */ @@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: @@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: From 91e1680c1bb45bceb1d1dd675782111713f76a8a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Jun 2013 17:13:34 +0200 Subject: [PATCH 04/10] fixed attitude estimator params --- .../attitude_estimator_ekf_params.c | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abdb..52dac652be 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,42 +44,42 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); + h->r0 = param_find("EKF_ATT_V3_R0"); + h->r1 = param_find("EKF_ATT_V3_R1"); + h->r2 = param_find("EKF_ATT_V3_R2"); + h->r3 = param_find("EKF_ATT_V3_R3"); - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); + h->roll_off = param_find("ATT_ROLL_OFF3"); + h->pitch_off = param_find("ATT_PITCH_OFF3"); + h->yaw_off = param_find("ATT_YAW_OFF3"); return OK; } From 8d0784af61536302cc6a2a1d65f847528658ba09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 6 Jul 2013 18:30:09 +0400 Subject: [PATCH 05/10] gpio_led: PX4IO RELAY and ACC outputs support, some fixes --- src/modules/gpio_led/gpio_led.c | 110 ++++++++++++++++++++++++++------ 1 file changed, 89 insertions(+), 21 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c74..632630b54a 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -54,9 +54,15 @@ #include #include +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + struct gpio_led_s { struct work_s work; int gpio_fd; + bool use_io; int pin; struct vehicle_status_s status; int vehicle_status_sub; @@ -75,51 +81,97 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { - int pin = GPIO_EXT_1; - if (argc < 2) { - errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]"); + errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" + "\t-p\tUse pin:\n" + "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" + "\t\t2\tPX4FMU GPIO_EXT2\n" + "\t\ta1\tPX4IO GPIO_ACC1\n" + "\t\ta2\tPX4IO GPIO_ACC2\n" + "\t\tr1\tPX4IO GPIO_RELAY1\n" + "\t\tr2\tPX4IO GPIO_RELAY2"); } else { - /* START COMMAND HANDLING */ if (!strcmp(argv[1], "start")) { + if (gpio_led_started) { + errx(1, "already running"); + } + + bool use_io = false; + int pin = GPIO_EXT_1; if (argc > 2) { - if (!strcmp(argv[1], "-p")) { - if (!strcmp(argv[2], "1")) { + if (!strcmp(argv[2], "-p")) { + if (!strcmp(argv[3], "1")) { + use_io = false; pin = GPIO_EXT_1; - } else if (!strcmp(argv[2], "2")) { + } else if (!strcmp(argv[3], "2")) { + use_io = false; pin = GPIO_EXT_2; + } else if (!strcmp(argv[3], "a1")) { + use_io = true; + pin = PX4IO_ACC1; + + } else if (!strcmp(argv[3], "a2")) { + use_io = true; + pin = PX4IO_ACC2; + + } else if (!strcmp(argv[3], "r1")) { + use_io = true; + pin = PX4IO_RELAY1; + + } else if (!strcmp(argv[3], "r2")) { + use_io = true; + pin = PX4IO_RELAY2; + } else { - warnx("[gpio_led] Unsupported pin: %s\n", argv[2]); - exit(1); + errx(1, "unsupported pin: %s", argv[3]); } } } memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.use_io = use_io; gpio_led_data.pin = pin; int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); if (ret != 0) { - warnx("[gpio_led] Failed to queue work: %d\n", ret); - exit(1); + errx(1, "failed to queue work: %d", ret); } else { gpio_led_started = true; + char pin_name[24]; + + if (use_io) { + if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); + + } else { + sprintf(pin_name, "PX4IO RELAY%i", pin); + } + + } else { + sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); + + } + + warnx("start, using pin: %s", pin_name); } exit(0); - /* STOP COMMAND HANDLING */ } else if (!strcmp(argv[1], "stop")) { - gpio_led_started = false; + if (gpio_led_started) { + gpio_led_started = false; + warnx("stop"); - /* INVALID COMMAND */ + } else { + errx(1, "not running"); + } } else { errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]); @@ -131,11 +183,22 @@ void gpio_led_start(FAR void *arg) { FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + char *gpio_dev; + + if (priv->use_io) { + gpio_dev = "/dev/px4io"; + + } else { + gpio_dev = "/dev/px4fmu"; + } + /* open GPIO device */ - priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + priv->gpio_fd = open(gpio_dev, 0); if (priv->gpio_fd < 0) { - warnx("[gpio_led] GPIO: open fail\n"); + // TODO find way to print errors + //printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev); + gpio_led_started = false; return; } @@ -150,11 +213,11 @@ void gpio_led_start(FAR void *arg) int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); if (ret != 0) { - warnx("[gpio_led] Failed to queue work: %d\n", ret); + // TODO find way to print errors + //printf("gpio_led: failed to queue work: %d\n", ret); + gpio_led_started = false; return; } - - warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); } void gpio_led_cycle(FAR void *arg) @@ -211,7 +274,12 @@ void gpio_led_cycle(FAR void *arg) if (priv->counter > 5) priv->counter = 0; - /* repeat cycle at 5 Hz*/ - if (gpio_led_started) + /* repeat cycle at 5 Hz */ + if (gpio_led_started) { work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); + + } else { + /* switch off LED on stop */ + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); + } } From 369e6d1eeab8478f3ce81a7803e55047c221d15b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 6 Jul 2013 18:37:02 +0400 Subject: [PATCH 06/10] gpio_led: minor usage fix --- src/modules/gpio_led/gpio_led.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 632630b54a..8b4c0cb308 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -86,10 +86,10 @@ int gpio_led_main(int argc, char *argv[]) "\t-p\tUse pin:\n" "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" "\t\t2\tPX4FMU GPIO_EXT2\n" - "\t\ta1\tPX4IO GPIO_ACC1\n" - "\t\ta2\tPX4IO GPIO_ACC2\n" - "\t\tr1\tPX4IO GPIO_RELAY1\n" - "\t\tr2\tPX4IO GPIO_RELAY2"); + "\t\ta1\tPX4IO ACC1\n" + "\t\ta2\tPX4IO ACC2\n" + "\t\tr1\tPX4IO RELAY1\n" + "\t\tr2\tPX4IO RELAY2"); } else { From e3bb9e87e2af2f0f70e486405e2c6df5d3e23667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 17:36:24 +0200 Subject: [PATCH 07/10] Hotfix for GPS: Disable unknown message classes --- src/drivers/gps/ubx.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67dc..b460890734 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,7 +176,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 1); + 0); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -327,6 +327,7 @@ UBX::parse_char(uint8_t b) } break; case UBX_DECODE_GOT_CLASS: + { add_byte_to_checksum(b); switch (_message_class) { case NAV: @@ -413,6 +414,14 @@ UBX::parse_char(uint8_t b) // config_needed = true; break; } + // Evaluate state machine - if the state changed, + // the state machine was reset via decode_init() + // and we want to tell the module to stop sending this message + + // disable unknown message + warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + configure_message_rate(_message_class, b, 0); + } break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); From c7f372916c5f74a3f9500a87d757abbaec3e1cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 18:25:42 +0200 Subject: [PATCH 08/10] Hotfix: Updated a MAVLink header --- mavlink/include/mavlink/v1.0/mavlink_conversions.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index 1dc9088b7a..d893635770 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -1,6 +1,12 @@ #ifndef _MAVLINK_CONVERSIONS_H_ #define _MAVLINK_CONVERSIONS_H_ +/* enable math defines on Windows */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif #include /** From 6cbbb9b99fbeddc2da00f67bb209d33971a30041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:46:24 +0200 Subject: [PATCH 09/10] Hotfix for GPS driver --- src/drivers/gps/ubx.cpp | 51 +++++++++++++++++++++-------------------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b460890734..3e790499be 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -224,35 +224,18 @@ UBX::receive(unsigned timeout) fds[0].fd = _fd; fds[0].events = POLLIN; - uint8_t buf[32]; + uint8_t buf[128]; /* timeout additional to poll */ uint64_t time_started = hrt_absolute_time(); - int j = 0; ssize_t count = 0; + bool message_found = false; + while (true) { - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message() > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { - return -1; - } - j++; - } - - /* everything is read */ - j = count = 0; - - /* then poll for new data */ + /* poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { @@ -272,8 +255,26 @@ UBX::receive(unsigned timeout) * available, we'll go back to poll() again... */ count = ::read(_fd, buf, sizeof(buf)); + /* pass received bytes to the packet decoder */ + for (int i = 0; i < count; i++) { + if (parse_char(buf[i])) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + handle_message(); + message_found = true; + } + } } } + + /* return success after receiving a packet */ + if (message_found) + return 1; + + /* abort after timeout if no packet parsed successfully */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } } } From c3d07030dd1882c626ed027cfc5870f42b1cd33e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:56:02 +0200 Subject: [PATCH 10/10] Minor additions to fix, pushing --- src/drivers/gps/ubx.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 3e790499be..ad219cd259 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -231,7 +231,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool message_found = false; + bool position_updated = false; while (true) { @@ -260,15 +260,15 @@ UBX::receive(unsigned timeout) if (parse_char(buf[i])) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - handle_message(); - message_found = true; + if (handle_message()) + position_updated = true; } } } } /* return success after receiving a packet */ - if (message_found) + if (position_updated) return 1; /* abort after timeout if no packet parsed successfully */ @@ -420,8 +420,8 @@ UBX::parse_char(uint8_t b) // and we want to tell the module to stop sending this message // disable unknown message - warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - configure_message_rate(_message_class, b, 0); + //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + //configure_message_rate(_message_class, b, 0); } break; case UBX_DECODE_GOT_MESSAGEID: