From 5375bb5b86e266157ceceef08c367da711b8144e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Jun 2013 01:04:32 +0200 Subject: Cleanup, WIP, needs a NuttX checkout to Firmware/NuttX now --- src/drivers/drv_gpio.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 92d184326..7e3998fca 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -42,7 +42,7 @@ #include -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* * PX4FMU GPIO numbers. * @@ -67,7 +67,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* * PX4IO GPIO numbers. * -- cgit v1.2.3 From 4db739b5e19953b5f6c4a16b2ddac8cdc0ae6634 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Jun 2013 01:48:42 +0200 Subject: Integration WIP with current NuttX version --- src/drivers/boards/px4fmu/px4fmu_init.c | 2 +- src/drivers/boards/px4fmu/px4fmu_internal.h | 2 +- src/drivers/boards/px4fmu/px4fmu_led.c | 2 +- src/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 2 +- src/drivers/boards/px4fmu/px4fmu_spi.c | 2 +- src/drivers/boards/px4fmu/px4fmu_usb.c | 2 +- src/drivers/boards/px4io/px4io_init.c | 2 +- src/drivers/boards/px4io/px4io_internal.h | 2 +- src/drivers/boards/px4io/px4io_pwm_servo.c | 2 +- src/drivers/stm32/adc/adc.cpp | 2 +- src/drivers/stm32/drv_hrt.c | 2 +- src/drivers/stm32/drv_pwm_servo.c | 2 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- src/modules/px4iofirmware/adc.c | 2 +- src/modules/systemlib/systemlib.h | 4 ++-- src/systemcmds/preflight_check/preflight_check.c | 13 +++++++++++-- 16 files changed, 27 insertions(+), 18 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 69edc23ab..212a92cfa 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -58,7 +58,7 @@ #include #include -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" #include "stm32_uart.h" diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h index 671a58f8f..383a046ff 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -50,7 +50,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ -#include +#include /**************************************************************************************************** diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c index 34fd194c3..31b25984e 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu/px4fmu_led.c @@ -41,7 +41,7 @@ #include -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" #include diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c index cb8918306..d85131dd8 100644 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c index b5d00eac0..e05ddecf3 100644 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -52,7 +52,7 @@ #include "up_arch.h" #include "chip.h" -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c index b0b669fbe..0be981c1e 100644 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -52,7 +52,7 @@ #include #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c index d36353c6f..15c59e423 100644 --- a/src/drivers/boards/px4io/px4io_init.c +++ b/src/drivers/boards/px4io/px4io_init.c @@ -54,7 +54,7 @@ #include -#include "stm32_internal.h" +#include "stm32.h" #include "px4io_internal.h" #include "stm32_uart.h" diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h index eb2820bb7..6638e715e 100644 --- a/src/drivers/boards/px4io/px4io_internal.h +++ b/src/drivers/boards/px4io/px4io_internal.h @@ -47,7 +47,7 @@ #include #include -#include +#include /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c index a2f73c429..6df470da6 100644 --- a/src/drivers/boards/px4io/px4io_pwm_servo.c +++ b/src/drivers/boards/px4io/px4io_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 911def943..1020eb946 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -58,7 +58,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index fd63681e3..05e1cd8ec 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -66,7 +66,7 @@ #include "up_internal.h" #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" #include "stm32_gpio.h" #include "stm32_tim.h" diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index c1efb8515..7b060412c 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -64,7 +64,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ac5511e60..167ef30a8 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -111,7 +111,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index f744698be..81566eb2a 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2c53c648b..ff9328b90 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include #include -__BEGIN_DECLS - /** Reboots the board */ extern void up_systemreset(void) noreturn_function; +__BEGIN_DECLS + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4bcce18fb..7752ffe67 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -146,6 +146,15 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = true; char nbuf[20]; + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + for (int i = 0; i < 12; i++) { /* should the channel be enabled? */ uint8_t count = 0; @@ -209,8 +218,8 @@ int preflight_check_main(int argc, char *argv[]) count++; } - /* XXX needs inspection of all the _MAP params */ - // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); // /* give system time to flush error message in case there are more */ // usleep(100000); -- cgit v1.2.3 From 23a62342359ba99eb1c6bb1832ba266b442a7e3e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Jun 2013 23:31:53 +0200 Subject: Rename our 'task_spawn' to 'task_spawn_cmd' since NuttX now has its own version of task_spawn that's different. --- src/drivers/ardrone_interface/ardrone_interface.c | 2 +- src/drivers/hil/hil.cpp | 2 +- src/drivers/hott_telemetry/hott_telemetry_main.c | 2 +- src/drivers/md25/md25_main.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/examples/fixedwing_control/main.c | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/modules/att_pos_estimator_ekf/kalman_main.cpp | 2 +- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.c | 2 +- src/modules/fixedwing_att_control/fixedwing_att_control_main.c | 2 +- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 2 +- src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c | 2 +- src/modules/mavlink/mavlink.c | 2 +- src/modules/mavlink_onboard/mavlink.c | 2 +- src/modules/multirotor_att_control/multirotor_att_control_main.c | 2 +- src/modules/multirotor_pos_control/multirotor_pos_control.c | 4 ++-- src/modules/position_estimator_mc/position_estimator_mc_main.c | 2 +- src/modules/sdlog/sdlog.c | 4 ++-- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/systemlib.c | 2 +- src/modules/systemlib/systemlib.h | 2 +- 23 files changed, 25 insertions(+), 25 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index aeaf830de..735bdb41a 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -114,7 +114,7 @@ int ardrone_interface_main(int argc, char *argv[]) } thread_should_exit = false; - ardrone_interface_task = task_spawn("ardrone_interface", + ardrone_interface_task = task_spawn_cmd("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 2048, diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4..bd027ce0b 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -224,7 +224,7 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = task_spawn("fmuhil", + _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index a13a6ef58..60f3f3e96 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -272,7 +272,7 @@ int hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("hott_telemetry", + deamon_task = task_spawn_cmd("hott_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 2048, diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 80850e708..e62c46b0d 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -109,7 +109,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("md25", + deamon_task = task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..b54b7aba1 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -313,7 +313,7 @@ MK::init(unsigned motors) gpio_reset(); /* start the IO interface task */ - _task = task_spawn("mkblctrl", + _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892eb..5147ac500 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -239,7 +239,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = task_spawn("fmuservo", + _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 89fbef020..27888523b 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -528,7 +528,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("ex_fixedwing_control", + deamon_task = task_spawn_cmd("ex_fixedwing_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index a5d847777..fbb38e4b9 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -91,7 +91,7 @@ int px4_deamon_app_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("deamon", + deamon_task = task_spawn_cmd("deamon", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index aebe3d1fe..3b411031a 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -100,7 +100,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("kalman_demo", + deamon_task = task_spawn_cmd("kalman_demo", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4096, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 8e18c3c9a..7e3eca085 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -123,7 +123,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 12400, diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04..67f053e22 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1200,7 +1200,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn("commander", + daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3000, diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 58477632b..6c9c137bb 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -336,7 +336,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("fixedwing_att_control", + deamon_task = task_spawn_cmd("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index e21990c92..e2330427d 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -107,7 +107,7 @@ int fixedwing_backside_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("control_demo", + deamon_task = task_spawn_cmd("control_demo", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 71c78f5b8..48c0b9f9d 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -448,7 +448,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("fixedwing_pos_control", + deamon_task = task_spawn_cmd("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index de78cd139..c72a53fee 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -787,7 +787,7 @@ int mavlink_main(int argc, char *argv[]) errx(0, "mavlink already running\n"); thread_should_exit = false; - mavlink_task = task_spawn("mavlink", + mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 5a2685560..cb6d6b16a 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -495,7 +495,7 @@ int mavlink_onboard_main(int argc, char *argv[]) errx(0, "mavlink already running\n"); thread_should_exit = false; - mavlink_task = task_spawn("mavlink_onboard", + mavlink_task = task_spawn_cmd("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c..99f25cfe9 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -466,7 +466,7 @@ int multirotor_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1 + optioncount], "start")) { thread_should_exit = false; - mc_task = task_spawn("multirotor_att_control", + mc_task = task_spawn_cmd("multirotor_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 2048, diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8..f39d11438 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -94,7 +94,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int multirotor_pos_control_main(int argc, char *argv[]) { @@ -110,7 +110,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn_cmd("multirotor pos control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 10dee3f22..984bd1329 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -123,7 +123,7 @@ int position_estimator_mc_main(int argc, char *argv[]) } thread_should_exit = false; - position_estimator_mc_task = task_spawn("position_estimator_mc", + position_estimator_mc_task = task_spawn_cmd("position_estimator_mc", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c index 84a9eb6ac..c22523bf2 100644 --- a/src/modules/sdlog/sdlog.c +++ b/src/modules/sdlog/sdlog.c @@ -161,7 +161,7 @@ bool logging_enabled = true; * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int sdlog_main(int argc, char *argv[]) { @@ -177,7 +177,7 @@ int sdlog_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("sdlog", + deamon_task = task_spawn_cmd("sdlog", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, 4096, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee..8bf5fdbd0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1445,7 +1445,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn("sensors_task", + _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 7814950fc..3283aad8a 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -65,7 +65,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index ff9328b90..0194b5e52 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -58,7 +58,7 @@ __EXPORT void killall(void); #endif /** Starts a task and performs any specific accounting, scheduler setup, etc. */ -__EXPORT int task_spawn(const char *name, +__EXPORT int task_spawn_cmd(const char *name, int priority, int scheduler, int stack_size, -- cgit v1.2.3 From 8eb4a03274b3012f5631f9a25f3a3f98f4d19159 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Jun 2013 23:58:06 -0700 Subject: Use a better way of guessing whether we can use both-edges mode. --- src/drivers/stm32/drv_hrt.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 05e1cd8ec..7ef3db970 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -278,8 +278,10 @@ static void hrt_call_invoke(void); #ifdef CONFIG_HRT_PPM /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + * + * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ -# ifndef GTIM_CCER_CC1NP +# ifdef CONFIG_ARCH_CORTEXM3 # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 -- cgit v1.2.3 From 3163d7ac0908dfee0978992137500f11f8a42c43 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 16 Jun 2013 22:41:08 -0700 Subject: Set the serial port speed before trying to talk to IO --- src/drivers/px4io/uploader.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 15524c3ae..c2f9979b0 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include "uploader.h" @@ -121,6 +122,12 @@ PX4IO_Uploader::upload(const char *filenames[]) return -errno; } + /* adjust line speed to match bootloader */ + struct termios t; + tcgetattr(_io_fd, &t); + cfsetspeed(&t, 115200); + tcsetattr(_io_fd, TCSANOW, &t); + /* look for the bootloader */ ret = sync(); @@ -256,7 +263,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) } read(_io_fd, &c, 1); - //log("recv 0x%02x", c); + log("recv 0x%02x", c); return OK; } @@ -283,7 +290,7 @@ PX4IO_Uploader::drain() ret = recv(c, 1000); if (ret == OK) { - //log("discard 0x%02x", c); + log("discard 0x%02x", c); } } while (ret == OK); } @@ -291,7 +298,7 @@ PX4IO_Uploader::drain() int PX4IO_Uploader::send(uint8_t c) { - //log("send 0x%02x", c); + log("send 0x%02x", c); if (write(_io_fd, &c, 1) != 1) return -errno; -- cgit v1.2.3 From b2ff8b5e1ad80f86aa2efdfdf06b7c55de8d32c0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 16 Jun 2013 23:02:46 -0700 Subject: Turn off logging --- src/drivers/px4io/uploader.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index c2f9979b0..9e3f041e3 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -258,12 +258,12 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { - log("poll timeout %d", ret); + //log("poll timeout %d", ret); return -ETIMEDOUT; } read(_io_fd, &c, 1); - log("recv 0x%02x", c); + //log("recv 0x%02x", c); return OK; } @@ -290,7 +290,7 @@ PX4IO_Uploader::drain() ret = recv(c, 1000); if (ret == OK) { - log("discard 0x%02x", c); + //log("discard 0x%02x", c); } } while (ret == OK); } @@ -298,7 +298,7 @@ PX4IO_Uploader::drain() int PX4IO_Uploader::send(uint8_t c) { - log("send 0x%02x", c); + //log("send 0x%02x", c); if (write(_io_fd, &c, 1) != 1) return -errno; -- cgit v1.2.3 From 209dc7100e03c257a88d3c71221f136295d61929 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 2 Jul 2013 19:46:15 +0200 Subject: mkclctrl 8/11Bit support, uOrb Topic added, ESC Topic to Sdlog2 added --- src/drivers/mkblctrl/mkblctrl.cpp | 125 ++++++++++++++++++++++++++--------- src/modules/sdlog2/sdlog2.c | 33 ++++++++- src/modules/sdlog2/sdlog2_messages.h | 18 +++++ src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/esc_status.h | 114 ++++++++++++++++++++++++++++++++ 5 files changed, 262 insertions(+), 31 deletions(-) create mode 100644 src/modules/uORB/topics/esc_status.h (limited to 'src/drivers') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..d0a0829b0 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -87,7 +88,7 @@ #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 2500 - +#define ESC_UORB_PUBLISH_DELAY 200 class MK : public device::I2C { @@ -119,6 +120,7 @@ public: int set_pwm_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); + int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); @@ -136,11 +138,15 @@ private: unsigned int _motor; int _px4mode; int _frametype; + orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; + orb_advert_t _t_esc_status; + unsigned int _num_outputs; bool _primary_pwm_device; bool _motortest; + bool _overrideSecurityChecks; volatile bool _task_should_exit; bool _armed; @@ -214,6 +220,7 @@ struct MotorData_t { unsigned int Version; // the version of the BL (0 = old) unsigned int SetPoint; // written by attitude controller unsigned int SetPointLowerBits; // for higher Resolution of new BLs + float SetPoint_PX4; // Values from PX4 unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present unsigned int ReadMode; // select data to read unsigned short RawPwmValue; // length of PWM pulse @@ -243,8 +250,10 @@ MK::MK(int bus) : _t_armed(-1), _t_outputs(0), _t_actuators_effective(0), + _t_esc_status(0), _num_outputs(0), _motortest(false), + _overrideSecurityChecks(false), _motor(-1), _px4mode(MAPPING_MK), _frametype(FRAME_PLUS), @@ -464,6 +473,13 @@ MK::set_motor_test(bool motortest) return OK; } +int +MK::set_overrideSecurityChecks(bool overrideSecurityChecks) +{ + _overrideSecurityChecks = overrideSecurityChecks; + return OK; +} + short MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) { @@ -506,8 +522,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - - /* advertise the effective control inputs */ actuator_controls_effective_s controls_effective; memset(&controls_effective, 0, sizeof(controls_effective)); @@ -515,6 +529,12 @@ MK::task_main() _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), &controls_effective); + /* advertise the blctrl status */ + esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + _t_esc_status = orb_advertise(ORB_ID(esc_status), &esc); + + pollfd fds[2]; fds[0].fd = _t_actuators; @@ -602,9 +622,11 @@ MK::task_main() } } - /* don't go under BLCTRL_MIN_VALUE */ - if (outputs.output[i] < BLCTRL_MIN_VALUE) { - outputs.output[i] = BLCTRL_MIN_VALUE; + if(!_overrideSecurityChecks) { + /* don't go under BLCTRL_MIN_VALUE */ + if (outputs.output[i] < BLCTRL_MIN_VALUE) { + outputs.output[i] = BLCTRL_MIN_VALUE; + } } /* output to BLCtrl's */ @@ -612,7 +634,10 @@ MK::task_main() mk_servo_test(i); } else { - mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine + //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine + // 11 Bit + Motor[i].SetPoint_PX4 = outputs.output[i]; + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine } } @@ -635,8 +660,43 @@ MK::task_main() } + + /* + * Only update esc topic every half second. + */ + + if (hrt_absolute_time() - esc.timestamp > 500000) { + esc.counter++; + esc.timestamp = hrt_absolute_time(); + esc.esc_count = (uint8_t) _num_outputs; + esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C; + + for (unsigned int i = 0; i < _num_outputs; i++) { + esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; + esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; + esc.esc[i].esc_version = (uint16_t) Motor[i].Version; + esc.esc[i].esc_voltage = (uint16_t) 0; + esc.esc[i].esc_current = (uint16_t) Motor[i].Current; + esc.esc[i].esc_rpm = (uint16_t) 0; + esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; + if (Motor[i].Version == 1) { + // BLCtrl 2.0 (11Bit) + esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits; + } else { + // BLCtrl < 2.0 (8Bit) + esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; + } + esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; + esc.esc[i].esc_state = (uint16_t) Motor[i].State; + esc.esc[i].esc_errorcount = (uint16_t) 0; + } + + orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } + } + //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); ::close(_t_armed); @@ -715,17 +775,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput) fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { - _task_should_exit = true; + + if(!_overrideSecurityChecks) { + if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { + _task_should_exit = true; + } } } return foundMotorCount; } - - - int MK::mk_servo_set(unsigned int chan, short val) { @@ -738,17 +798,15 @@ MK::mk_servo_set(unsigned int chan, short val) tmpVal = val; - if (tmpVal > 1024) { - tmpVal = 1024; + if (tmpVal > 2047) { + tmpVal = 2047; } else if (tmpVal < 0) { tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - //Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4; - - Motor[chan].SetPointLowerBits = 0; + Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff; + Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07; if (_armed == false) { Motor[chan].SetPoint = 0; @@ -1014,8 +1072,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; - mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024)); - + mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); } else { ret = -EINVAL; } @@ -1112,25 +1169,19 @@ ssize_t MK::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - // loeschen uint16_t values[4]; uint16_t values[8]; - // loeschen if (count > 4) { - // loeschen // we only have 4 PWM outputs on the FMU - // loeschen count = 4; - // loeschen } if (count > _num_outputs) { // we only have 8 I2C outputs in the driver count = _num_outputs; } - // allow for misaligned values memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { Motor[i].RawPwmValue = (unsigned short)values[i]; - mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024)); + mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047)); } return count * 2; @@ -1282,7 +1333,7 @@ enum FrameType { PortMode g_port_mode; int -mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) +mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { uint32_t gpio_bits; int shouldStop = 0; @@ -1341,6 +1392,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* motortest if enabled */ g_mk->set_motor_test(motortest); + /* ovveride security checks if enabled */ + g_mk->set_overrideSecurityChecks(overrideSecurityChecks); + /* count used motors */ do { @@ -1406,6 +1460,7 @@ mkblctrl_main(int argc, char *argv[]) int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; + bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; @@ -1461,11 +1516,21 @@ mkblctrl_main(int argc, char *argv[]) showHelp = true; } + /* look for the optional --override-security-checks parameter */ + if (strcmp(argv[i], "--override-security-checks") == 0) { + overrideSecurityChecks = true; + newMode = true; + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); + fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); exit(1); } @@ -1483,7 +1548,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype); + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); } /* test, etc. here g*/ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4cca46b9c..5cc80ead6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -79,6 +79,7 @@ #include #include #include +#include #include @@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 17; + const ssize_t fdsc = 18; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct rc_channels_s rc; struct differential_pressure_s diff_pres; struct airspeed_s airspeed; + struct esc_status_s esc; } buf; memset(&buf, 0, sizeof(buf)); @@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int flow_sub; int rc_sub; int airspeed_sub; + int esc_sub; } subs; /* log message buffer: header + body */ @@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; + struct log_ESC_s log_ESC; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- ESCs --- */ + subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); + fds[fdsc_count].fd = subs.esc_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1105,6 +1115,27 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(AIRS); } + /* --- ESCs --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); + for (uint8_t i=0; i + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file esc_status.h + * Definition of the esc_status uORB topic. + * + * Published the state machine and the system status bitfields + * (see SYS_STATUS mavlink message), published only by commander app. + * + * All apps should write to subsystem_info: + * + * (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app) + */ + +#ifndef ESC_STATUS_H_ +#define ESC_STATUS_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + +/** + * The number of ESCs supported. + * Current (Q2/2013) we support 8 ESCs, + */ +#define CONNECTED_ESC_MAX 8 + +enum ESC_VENDOR { + ESC_VENDOR_GENERIC = 0, /**< generic ESC */ + ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */ +}; + +enum ESC_CONNECTION_TYPE { + ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */ + ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */ + ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */ + ESC_CONNECTION_TYPE_I2C, /**< I2C */ + ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */ +}; + +/** + * + */ +struct esc_status_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + uint8_t esc_count; /**< number of connected ESCs */ + enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ + + struct { + uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ + enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ + uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ + uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ + uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ + uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + } esc[CONNECTED_ESC_MAX]; + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +//ORB_DECLARE(esc_status); +ORB_DECLARE_OPTIONAL(esc_status); + +#endif -- cgit v1.2.3 From 01255a4cec9683d05263ea4509bd324b7b814156 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 7 Jul 2013 01:10:47 +0200 Subject: Remove the <15kmh cuttoff and report kmh via HoTT. --- src/drivers/ets_airspeed/ets_airspeed.cpp | 5 +++-- src/drivers/hott_telemetry/messages.c | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index c39da98d7..b34d3fa5d 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -84,8 +84,9 @@ /** * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * You can set this value to 12 if you want a zero reading below 15km/h. */ -#define MIN_ACCURATE_DIFF_PRES_PA 12 +#define MIN_ACCURATE_DIFF_PRES_PA 0 /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ @@ -463,8 +464,8 @@ ETSAirspeed::collect() uint16_t diff_pres_pa = val[1] << 8 | val[0]; + // XXX move the parameter read out of the driver. param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index d2634ef41..0ce56acef 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -108,7 +108,7 @@ build_eam_response(uint8_t *buffer, size_t *size) memset(&airspeed, 0, sizeof(airspeed)); orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); - uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; -- cgit v1.2.3