From 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: commander: Added time of RC-loss/gain to the mavlink_log_critical message to better inform the user --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df..23fdcb414 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; status_changed = true; } -- cgit v1.2.3 From 4afa8645832c71ea1c35dc73d9424df80fac531a Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 16:25:38 +0200 Subject: commander: Added duration of rc-loss to mavlink_log_critical message --- src/modules/commander/commander.cpp | 3 ++- src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 23fdcb414..d8ebee4b6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1594,6 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e3..8d797e21a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ -- cgit v1.2.3 From 4656db01c215c94dda70d7b579037ec368d3275e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Oct 2014 17:59:53 +0200 Subject: Enable IO safety parameter --- src/drivers/px4io/px4io.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be766..821eb52ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -817,6 +817,11 @@ PX4IO::init() } + /* set safety to off if circuit breaker enabled */ + if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); -- cgit v1.2.3 From c7a3a0db5230d1506c81c5f15254002b64cddf32 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 31 Oct 2014 08:58:58 +0100 Subject: Don't go into an error state if we are temporarily powering via USB on the bench --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b72ebcc50..46caddd46 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1418,7 +1418,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; -- cgit v1.2.3 From eeda1886f0e7f15b2e51d863020f794b1a84efc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:07:36 +0100 Subject: Simplify error messages for NSH term --- src/systemcmds/nshterm/nshterm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index fca1798e6..f06c49552 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -87,7 +87,7 @@ nshterm_main(int argc, char *argv[]) /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + warnx("ERR get config %s: %d\n", argv[1], termios_state); close(fd); return -1; } @@ -96,7 +96,7 @@ nshterm_main(int argc, char *argv[]) uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + warnx("ERR set config %s\n", argv[1]); close(fd); return -1; } -- cgit v1.2.3 From 83eb81251c8405c96aaa72fc1634d15a5b54af29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:22:18 +0100 Subject: NSH term: Only time out if no arming information is available, if arming information is available abort if unconnected on arming --- src/systemcmds/nshterm/nshterm.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index f06c49552..edeb5c624 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -51,6 +51,8 @@ #include #include +#include + __EXPORT int nshterm_main(int argc, char *argv[]); int @@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[]) } unsigned retries = 0; int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + /* we assume the system does not provide arming status feedback */ + bool armed_updated = false; + + /* try the first 30 seconds or if arming system is ready */ + while ((retries < 300) || armed_updated) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + if (orb_check(armed_fd, &updated)) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + armed_updated = true; + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* try the first 30 seconds */ - while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); -- cgit v1.2.3 From c442f820dd89aad9fac394ce56c07b10cfcacf73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:54:11 +0100 Subject: Encode RC type in RSSI field for GCS --- src/modules/mavlink/mavlink_messages.cpp | 43 +++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..87858690f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1717,7 +1717,48 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } -- cgit v1.2.3 From 412ddde5dca055ba090ed2e18db2dc77b6af93a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:58:12 +0100 Subject: Force RSSI to zero if RC is lost --- src/modules/mavlink/mavlink_messages.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 87858690f..a2b3cc62e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1760,6 +1760,11 @@ protected: break; } + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } -- cgit v1.2.3 From 2cadd45f307750bb68c3f926d7d9830f680bd94b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 10:13:11 +0100 Subject: Configure led ring and enable heartbeat on it --- src/drivers/boards/px4io-v2/board_config.h | 1 + src/drivers/boards/px4io-v2/px4iov2_init.c | 1 + src/modules/px4iofirmware/px4io.c | 2 ++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 5 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index ef9bb5cad..10a93be0b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -77,6 +77,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) /* Safety switch button *******************************************************/ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 9f8c0eeb2..5c3343ccc 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_LED4); stm32_configgpio(GPIO_BTN_SAFETY); diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd134ccb4..f978ff406 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,6 +124,7 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); + LED_RING(heartbeat); } static uint64_t reboot_time; @@ -191,6 +192,7 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); + LED_RING(false); /* turn on servo power (if supported) */ #ifdef POWER_SERVO diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e32fcc72b..8186e4c78 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -- cgit v1.2.3 From 75d0ffe4e5b7659e20bbbdcc1e840e06dfe7a138 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:34:33 +0100 Subject: Build fix --- src/modules/px4iofirmware/px4io.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index f978ff406..30f32b38e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,7 +124,9 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +#ifdef GPIO_LED4 LED_RING(heartbeat); +#endif } static uint64_t reboot_time; @@ -192,7 +194,9 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); +#ifdef GPIO_LED4 LED_RING(false); +#endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO -- cgit v1.2.3 From ce1ec430f89e2080ad053115459bf91dd6585d3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:58:32 +0100 Subject: Ensure MAVLink app has enough stack space --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f..6b4edff78 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1634,7 +1634,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2900, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From acb739655d5c2ebf50449842ae2b7b9b7c76dbd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 17:06:35 +0100 Subject: Remove huge memory overhead in RC channels topic, was completely unnecessary --- src/modules/uORB/topics/rc_channels.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 8978de471..b14ae1edd 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -76,7 +76,6 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ uint8_t channel_count; /**< Number of valid channels */ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ uint8_t rssi; /**< Receive signal strength index */ bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ -- cgit v1.2.3 From baba157785ee1298e70fb358a0ffe76f18fc3b54 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 1 Nov 2014 18:45:40 -0400 Subject: Encoder logging support. --- src/modules/sdlog2/sdlog2.c | 16 +++++++++++++++- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde37432..1820fc104 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include #include #include +#include #include #include @@ -954,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -996,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCODERS_s log_ENCODERS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1033,6 +1036,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1064,7 +1068,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); - + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); /* add new topics HERE */ @@ -1667,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCODERS_MSG; + log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; + log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; + log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..7fd07809d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -407,6 +407,16 @@ struct log_VISN_s { float qw; }; +/* --- ENCODERS - ENCODER DATA --- */ +#define LOG_ENCODERS_MSG 39 +struct log_ENCODERS_s { + int64_t counts_0; + float velocity_0; + int64_t counts_1; + float velocity_1; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), + LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 0e5b91d16fd35b1ad78df1a772ceb23676e5fa7a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 2 Nov 2014 14:22:23 -0500 Subject: Shortened encoder logging names. --- src/modules/sdlog2/sdlog2.c | 14 +++++++------- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1820fc104..af580f1f7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -998,7 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; - struct log_ENCODERS_s log_ENCODERS; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1673,12 +1673,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ENCODERS --- */ if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { - log_msg.msg_type = LOG_ENCODERS_MSG; - log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; - log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; - log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; - log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; - LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7fd07809d..fa9bdacb8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -408,12 +408,12 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCODERS_MSG 39 -struct log_ENCODERS_s { - int64_t counts_0; - float velocity_0; - int64_t counts_1; - float velocity_1; +#define LOG_ENCD_MSG 39 +struct log_ENCD_s { + int64_t cnt0; + float vel0; + int64_t cnt1; + float vel1; }; @@ -481,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), - LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), + LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 82ebf3ca1c46dd228ed1a95fd334cae6af9dd252 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:23:02 +0100 Subject: Update NuttX, print stack trace on stack overflow handler --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 41fffa0df..5ee4b2b2c 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c +Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 -- cgit v1.2.3 From b51c6693445be6ff230b3d34ad400553aadddd7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:24:50 +0100 Subject: Commander: Improve calibration routines to produce more conscise and better sequenced instructions --- .../commander/accelerometer_calibration.cpp | 44 ++++++++++++++-------- src/modules/commander/airspeed_calibration.cpp | 25 ++++++++---- src/modules/commander/gyro_calibration.cpp | 5 ++- src/modules/commander/mag_calibration.cpp | 2 +- 4 files changed, 51 insertions(+), 25 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766..0cb41489f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); + continue; } + /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe..cae1d7684 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", @@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2b..8ab14dd52 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f386..7be8de9c6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; -- cgit v1.2.3 From 166580b8f7415de90f3e5c0171a75b9827158d12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:41:55 +0100 Subject: Time out on serial instead of just hanging there --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index b46db00b5..0bf2b82dd 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): + def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate) + self.port = serial.Serial(portname, baudrate, timeout=0.5) self.otp = b'' self.sn = b'' -- cgit v1.2.3 From 5862f4ffe6b463934cceb7bb034e77f100f97633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:00:42 +0100 Subject: Fix error handling --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 0bf2b82dd..ea2f64639 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data (%u bytes)", count) + raise RuntimeError("timeout waiting for data (%u bytes)" % count) # print("recv " + binascii.hexlify(c)) return c -- cgit v1.2.3 From 44a247363248caadecc4518048ab513d2dfd202e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:03:49 +0100 Subject: Fix up reboot logic --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index ea2f64639..8a89fdf06 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -511,6 +511,8 @@ while True: up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) + # always close the port + up.close() continue try: -- cgit v1.2.3 From 72977ee9098758b5e00d9ac8df916bb221b2953f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:13:59 +0100 Subject: Better error handling instructions --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8a89fdf06..d4e461226 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up while True: @@ -508,6 +509,7 @@ while True: except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) -- cgit v1.2.3 From 5e2330abbae35bfc45e5ac67d3590b53de20f526 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 08:39:16 +0100 Subject: Add USB load test from Mark Whitehorn --- Tools/usb_serialload.py | 55 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 Tools/usb_serialload.py diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py new file mode 100644 index 000000000..5c864532f --- /dev/null +++ b/Tools/usb_serialload.py @@ -0,0 +1,55 @@ +import serial, time + + +port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2) + +data = '01234567890123456789012345678901234567890123456789' +#data = 'hellohello' +outLine = 'echo %s\n' % data + +port.write('\n\n\n') +port.write('free\n') +line = port.readline(80) +while line != '': + print(line) + line = port.readline(80) + + +i = 0 +bytesOut = 0 +bytesIn = 0 + +startTime = time.time() +lastPrint = startTime +while True: + bytesOut += port.write(outLine) + line = port.readline(80) + bytesIn += len(line) + # check command line echo + if (data not in line): + print('command error %d: %s' % (i,line)) + #break + # read echo output + line = port.readline(80) + if (data not in line): + print('echo output error %d: %s' % (i,line)) + #break + bytesIn += len(line) + #print('%d: %s' % (i,line)) + #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn)) + + elapsedT = time.time() - lastPrint + if (time.time() - lastPrint >= 5): + outRate = bytesOut / elapsedT + inRate = bytesIn / elapsedT + usbRate = (bytesOut + bytesIn) / elapsedT + lastPrint = time.time() + print('elapsed time: %f' % (time.time() - startTime)) + print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate)) + + bytesOut = 0 + bytesIn = 0 + + i += 1 + #if (i > 2): break + -- cgit v1.2.3 From 9ac13745f8356db60322fa92ecdff1125c76f172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:06 +0100 Subject: Adjust MAVLink RX prio to ensure received data can still be processed --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9247f999a..bc092c7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1396,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); -- cgit v1.2.3 From 8652ee0b6474fa4fa92b0d6ab5eaec1436a03b8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:24 +0100 Subject: Drop NSH priority to keep system responsive --- src/systemcmds/nshterm/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 41706e137..a12bc369e 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -41,3 +41,5 @@ SRCS = nshterm.c MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os + +MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30" -- cgit v1.2.3 From 06df0f23a32c87486815a9794c1425fe4534ac13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 12:56:35 +0100 Subject: L3GD20: Output gyro temperature in report --- src/drivers/l3gd20/l3gd20.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cfae8761c..ff2871354 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,6 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#define L3GD20_TEMP_OFFSET_CELCIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -856,7 +857,7 @@ L3GD20::measure() #pragma pack(push, 1) struct { uint8_t cmd; - uint8_t temp; + int8_t temp; uint8_t status; int16_t x; int16_t y; @@ -930,6 +931,8 @@ L3GD20::measure() report.z_raw = raw_report.z; + report.temperature_raw = raw_report.temp; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; @@ -938,6 +941,8 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); @@ -1091,9 +1096,11 @@ test() warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("temp: \t%d\tC", (int)g_report.temperature); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("temp: \t%d\traw", (int)g_report.temperature_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); -- cgit v1.2.3 From c600a7fbd27c298e855d1f3e6f00f590141f995e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 15:37:24 +0100 Subject: L3GD20: Fix typo --- src/drivers/l3gd20/l3gd20.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ff2871354..82fa5cc6e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,7 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 -#define L3GD20_TEMP_OFFSET_CELCIUS 40 +#define L3GD20_TEMP_OFFSET_CELSIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -941,7 +941,7 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); - report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); -- cgit v1.2.3 From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 14 Aug 2014 15:43:27 +0200 Subject: UAVCAN: allow to arm single ESCs --- src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++----- src/modules/uavcan/actuators/esc.hpp | 9 ++++++++- src/modules/uavcan/uavcan_main.cpp | 2 +- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef..fbd4f0bcd 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + for (unsigned i = 0; i < num_outputs; i++) { + if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely @@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } + else { + msg.cmd.push_back(static_cast(0)); + } } /* @@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) +{ + if (arm) + _armed_mask = -1; + else + _armed_mask = 0; +} + +void UavcanEscController::arm_single_esc(int num, bool arm) { - _armed = arm; + if (arm) + _armed_mask = MOTOR_BIT(num); + else + _armed_mask = 0; } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e6..ff3ecfb21 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -98,6 +99,12 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + /* + * ESC states + */ + uint32_t _armed_mask = 0; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + /* * Perf counters */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..59f0f4300 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -429,7 +429,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } -- cgit v1.2.3 From a18460183cdfa590af29c2299acb30206b418cb1 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 19:37:44 +0100 Subject: motor_test: cleanup --- src/modules/uORB/topics/test_motor.h | 2 +- src/systemcmds/motor_test/motor_test.c | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index 491096934..c880fe099 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output data, in natural output units */ + float value; /**< output power, range [0..1] }; /** diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674..a73f46626 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -54,7 +54,8 @@ #include "systemlib/err.h" -__EXPORT int motor_test_main(int argc, char *argv[]); +__EXPORT int motor_test_main(int argc, char *argv[]); + static void motor_test(unsigned channel, float value); static void usage(const char *reason); @@ -67,13 +68,13 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + if (_test_motor_pub > 0) { + /* publish test state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } } static void usage(const char *reason) @@ -102,18 +103,18 @@ int motor_test_main(int argc, char *argv[]) switch (ch) { case 'm': - /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + /* Read in motor number */ channel = strtoul(optarg, NULL, 0); break; case 'p': - /* Read in custom low value */ + /* Read in power value */ lval = strtoul(optarg, NULL, 0); if (lval > 100) usage("value invalid"); - value = (float)lval/100.f; + value = ((float)lval)/100.f; break; default: usage(NULL); -- cgit v1.2.3 From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 18:47:07 +0100 Subject: UAVCAN: implemented motor testing --- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 28 ++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 5 +++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index ff3ecfb21..12c035542 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -103,7 +103,6 @@ private: * ESC states */ uint32_t _armed_mask = 0; - uavcan::equipment::esc::Status _states[MAX_ESCS]; /* * Perf counters diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 59f0f4300..653d4f98c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -279,6 +279,7 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -344,7 +345,14 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + float outputs[NUM_ACTUATOR_OUTPUTS] = {}; + outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + + // Output to the bus + _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); + } + else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +392,27 @@ int UavcanNode::run() } } - // Check arming state + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db9741..274321f0d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -103,6 +104,10 @@ private: actuator_armed_s _armed; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus + int _test_motor_sub = -1; ///< uORB subscription of the test_motor status + test_motor_s _test_motor; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer -- cgit v1.2.3 From f5dbfe63b3d7fbfcac524ba03028de962a8716e2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:52:29 +0100 Subject: added new sensor board rotation option (roll 270, yaw 270) --- src/lib/conversion/rotation.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 5187b448f..739c3a960 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,6 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; @@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 90 }, {270, 0, 135 }, { 0, 90, 0 }, - { 0, 270, 0 } + { 0, 270, 0 }, + {270, 0, 270 } }; /** -- cgit v1.2.3 From cbd20f48d670749fcbfe49c0f5135572bdf7ee44 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:57:09 +0100 Subject: fixed style --- src/lib/conversion/rotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 739c3a960..917c7f830 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,7 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, - ROTATION_ROLL_270_YAW_270 = 26, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; -- cgit v1.2.3 From 182c1c1d52ef370a42b99c825e3f45a03ecbcdc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 17:17:34 +0100 Subject: Fix RC mapping indices - 0 index induces issues right now --- src/drivers/px4io/px4io.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fd9eb4170..c6543982e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1247,28 +1247,33 @@ PX4IO::io_set_rc_config() */ param_get(param_find("RC_MAP_ROLL"), &ichan); + /* subtract one from 1-based index - this might be + * a negative number now + */ + ichan -= 1; + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 0; + input_map[ichan] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 1; + input_map[ichan] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 2; + input_map[ichan] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 3; + input_map[ichan] = 3; param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 4; + input_map[ichan] = 4; param_get(param_find("RC_MAP_MODE_SW"), &ichan); -- cgit v1.2.3 From 23a33e31cd91d074f2d16d04b0cd7f2c867d9d4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 19:26:10 +0100 Subject: Add missing mode switch channel --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c6543982e..6d68d9f60 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1279,7 +1279,7 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* -- cgit v1.2.3 From d907b030ee618b581e31964a34a61648e88793a3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 5 Nov 2014 13:15:52 -0800 Subject: Initialize RSSI so it doesn't remain uninitialized MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Spektrum doesn’t support rssi so it is not set by st24_decode. --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e..3fd73fc60 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -91,6 +92,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } -- cgit v1.2.3 From f414eef042be7ebf641638009b3676523bf4bed6 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 4 Nov 2014 11:10:14 +0100 Subject: TECS: Modify absolute-value limiting of throttle demand --- src/lib/external_lgpl/tecs/tecs.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0ed210cf4..cfcc48b62 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; - _throttle_dem = constrain(_throttle_dem, - _last_throttle_dem - thrRateIncr, - _last_throttle_dem + thrRateIncr); + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); } // Ensure _last_throttle_dem is always initialized properly - // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! _last_throttle_dem = _throttle_dem; - // Calculate integrator state upper and lower limits - // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand float integ_max = (_THRmaxf - _throttle_dem + 0.1f); float integ_min = (_THRminf - _throttle_dem - 0.1f); // Calculate integrator state, constraining state - // Set integrator to a max throttle value dduring climbout + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { -- cgit v1.2.3 From 3f3353f2c4ad9f7b8263379c8b31bd9a437b0174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:19:04 +1100 Subject: mixer: fixed stream handle leakage --- src/modules/systemlib/mixer/mixer_load.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50..0d629d610 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } -- cgit v1.2.3 From 43418a674903d242271028c4d4eb473f95a24be6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Nov 2014 13:31:33 +0100 Subject: application layer only, no drivers affected: Fix overflow in RC input topic - this topic is deprecated and will be removed, has been superseded by input_rc and manual_control --- src/modules/uORB/topics/rc_channels.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index b14ae1edd..16916cc4d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -34,6 +34,8 @@ /** * @file rc_channels.h * Definition of the rc_channels uORB topic. + * + * @deprecated DO NOT USE FOR NEW CODE */ #ifndef RC_CHANNELS_H_ @@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5, - RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ + AUX_5 }; +// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS + +#define RC_CHANNELS_FUNCTION_MAX 18 + /** * @addtogroup topics * @{ -- cgit v1.2.3 From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Wed, 29 Oct 2014 22:00:30 +1100 Subject: First attempt at sbus1 output. --- src/modules/px4iofirmware/sbus.c | 37 ++++++++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d61..23dae7898 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -80,8 +80,10 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; +static hrt_abstime last_txframe_time=0; static uint8_t frame[SBUS_FRAME_SIZE]; +static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -122,10 +124,39 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - char a = 'A'; - write(sbus_fd, &a, 1); -} + //int. first byte of data is offset 1 in sbus + uint8_t byteindex=1; + uint8_t offset=0; + uint16_t value; + hrt_abstime now; + //oframe[0]=0xf0; + oframe[0]=0x0f; + now = hrt_absolute_time(); + if ((now - last_txframe_time) > 14000) { + last_txframe_time = now; + + for (uint16_t i=1;i=8) { + ++byteindex; + offset-=8; + } + oframe[byteindex] |= (value<<(offset))&0xff; + oframe[byteindex+1]|=(value>>(8-offset))&0xff; + oframe[byteindex+2]|=(value>>(16-offset))&0xff; + offset+=11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); + } +} void sbus2_output(uint16_t *values, uint16_t num_values) { -- cgit v1.2.3 From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Sat, 8 Nov 2014 22:36:45 +1100 Subject: sbus1 output. Cleaned up. Safer bounds checking. --- src/modules/px4iofirmware/sbus.c | 45 +++++++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 23dae7898..2f234234e 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -57,6 +57,7 @@ #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 +#define SBUS1_FRAME_DELAY 14000 /* Measured values with Futaba FX-30/R6108SB: @@ -80,7 +81,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; -static hrt_abstime last_txframe_time=0; +static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; static uint8_t oframe[SBUS_FRAME_SIZE]; @@ -125,35 +126,41 @@ void sbus1_output(uint16_t *values, uint16_t num_values) { //int. first byte of data is offset 1 in sbus - uint8_t byteindex=1; - uint8_t offset=0; + uint8_t byteindex = 1; + uint8_t offset = 0; uint16_t value; hrt_abstime now; - //oframe[0]=0xf0; - oframe[0]=0x0f; + oframe[0] = 0x0f; - now = hrt_absolute_time(); - if ((now - last_txframe_time) > 14000) { + now = hrt_absolute_time(); + + if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - for (uint16_t i=1;i=8) { + if (value > 0x07ff ) { + value = 0x07ff; + } + + while (offset >= 8) { ++byteindex; - offset-=8; + offset -= 8; } - oframe[byteindex] |= (value<<(offset))&0xff; - oframe[byteindex+1]|=(value>>(8-offset))&0xff; - oframe[byteindex+2]|=(value>>(16-offset))&0xff; - offset+=11; - } + + oframe[byteindex] |= (value << (offset)) & 0xff; + oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff; + oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff; + offset += 11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); } } -- cgit v1.2.3 From 8599994ebb3e4ce63b5fe77cb1f2935d27604745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Nov 2014 20:38:57 +0100 Subject: Add generic uploader tool --- Tools/upload.sh | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100755 Tools/upload.sh diff --git a/Tools/upload.sh b/Tools/upload.sh new file mode 100755 index 000000000..17d87fe61 --- /dev/null +++ b/Tools/upload.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +EXEDIR=`pwd` +BASEDIR=$(dirname $0) + +SYSTYPE=`uname -s` + +# +# Serial port defaults. +# +# XXX The uploader should be smarter than this. +# +if [ $SYSTYPE=Darwin ]; +then +SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" +fi + +if [ $SYSTYPE=Linux ]; +then +SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" +fi + +if [ $SYSTYPE="" ]; +then +SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" +fi + +python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 \ No newline at end of file -- cgit v1.2.3 From e87a179fc1b9b10bfd22e741229e9cbd75b6ef73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 15:16:05 +0100 Subject: MAVLink update --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 90383fac8..ad5e5a645 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 90383fac84d031aef17989a1497c2473dfa64340 +Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30 -- cgit v1.2.3 From 62db84fa758ecc30081e30a18464fe3060d2c2d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 18:20:53 +0100 Subject: MAVLink update compile fixes --- src/modules/mavlink/mavlink.c | 6 +----- src/modules/mavlink/mavlink_main.cpp | 12 +++++++----- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_messages.cpp | 12 +++++++----- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bec706bad..9afe74af1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b4edff78..c251dd3b2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f96e586b..ad5e5001b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,6 +265,8 @@ public: struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + unsigned get_system_type() { return _system_type; } + protected: Mavlink *next; @@ -354,6 +356,8 @@ private: param_t _param_use_hil_gps; param_t _param_forward_externalsp; + unsigned _system_type; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..b4911427f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -302,7 +302,7 @@ protected: msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = mavlink_system.type; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; @@ -1353,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; -- cgit v1.2.3 From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 15:54:28 +1100 Subject: Code style cleanup. --- src/modules/px4iofirmware/sbus.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2f234234e..90c0e0503 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -125,8 +125,7 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - //int. first byte of data is offset 1 in sbus - uint8_t byteindex = 1; + uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; hrt_abstime now; @@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) oframe[i] = 0; } - // 16 is sbus number of servos/channels minus 2 single bit channels. - // currently ignoring single bit channels. - for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) { + /* 16 is sbus number of servos/channels minus 2 single bit channels. + * currently ignoring single bit channels. */ + + for (unsigned i = 0; (i < num_values) && (i < 16); ++i) { value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); - //protect from out of bounds values and limit to 11 bits; + + /*protect from out of bounds values and limit to 11 bits*/ if (value > 0x07ff ) { value = 0x07ff; } -- cgit v1.2.3 From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 16:34:28 +1100 Subject: Cleaned up sbus output frame initialisation. --- src/modules/px4iofirmware/sbus.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 90c0e0503..d76ec55f0 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -84,7 +84,6 @@ static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; -static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; - hrt_abstime now; - oframe[0] = 0x0f; + hrt_abstime now; now = hrt_absolute_time(); if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - - for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) { - oframe[i] = 0; - } + uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; /* 16 is sbus number of servos/channels minus 2 single bit channels. * currently ignoring single bit channels. */ -- cgit v1.2.3 From ea0a59f80650c7bd7c72889f1c7bb72bfd3fdbd5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 09:26:11 +0100 Subject: HMC5883: Reduce output, add indices to make back-tracking lines easier, remove output completely for an OK-operation (the driver is NOT the right place to talk to the user!) --- src/drivers/hmc5883/hmc5883.cpp | 49 +++++++++++++---------------------------- 1 file changed, 15 insertions(+), 34 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index e4ecfa6b5..81f767965 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) * LSM/Ga, giving 1.16 and 1.08 */ float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - warnx("starting mag scale calibration"); - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { - warn("failed to set 2Hz poll rate"); + warn("FAILED: SENSORIOCSPOLLRATE 2Hz"); ret = 1; goto out; } @@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) /* Set to 2.5 Gauss. We ask for 3 to get the right part of * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { - warnx("failed to set 2.5 Ga range"); + warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { - warnx("failed to enable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to get scale / offsets for mag"); + warn("FAILED: MAGIOCGSCALE 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 1"); ret = 1; goto out; } @@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 1"); goto out; } @@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 1"); ret = -EIO; goto out; } @@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 2"); goto out; } @@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 2"); ret = -EIO; goto out; } @@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sum_excited[1] += cal[1]; sum_excited[2] += cal[2]; } - - //warnx("periodic read %u", i); - //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } if (good_count < 5) { - warn("failed calibration"); ret = -EIO; goto out; } -#if 0 - warnx("measurement avg: %.6f %.6f %.6f", - (double)sum_excited[0]/good_count, - (double)sum_excited[1]/good_count, - (double)sum_excited[2]/good_count); -#endif - float scaling[3]; scaling[0] = sum_excited[0] / good_count; scaling[1] = sum_excited[1] / good_count; scaling[2] = sum_excited[2] / good_count; - warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; @@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("failed to set new scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 2"); } /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); + warnx("FAILED: MAGIOCSRANGE 1.1 Ga"); } if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 0"); } if (ret == OK) { - if (!check_scale()) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration finished with invalid results."); + if (check_scale()) { + /* failed */ + warnx("FAILED: SCALE"); ret = ERROR; } - } else { - warnx("mag scale calibration failed."); } return ret; -- cgit v1.2.3 From d253c8ba7d9cfc408328afd7c3708892b23626c0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:13 +1000 Subject: commander: correct the description array --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd46..1c3b68edd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -760,7 +760,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; -- cgit v1.2.3 From f7db91ac32d6db1fbef9aa0c868636561bd2282c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:46 +1000 Subject: commander: enhance the failsafe verbose output --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1c3b68edd..f9ea4e914 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1851,7 +1851,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + } failsafe_old = status.failsafe; } -- cgit v1.2.3 From f8bed3cd892157b2db147b0d6a60f26371acdeb8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:39:35 +1000 Subject: state_machine_helper: whitespace --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9568752ae..a49965efd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; -- cgit v1.2.3 From 2c577815818b579743dab13ef0055f02605699d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:40:13 +1000 Subject: state_machine_helper: trying to clean up some failsafe logic --- src/modules/commander/state_machine_helper.cpp | 43 ++++++++++++++++++-------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a49965efd..3553dcdc3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,11 +497,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink and the mission is finished */ + * - if the datalink is enabled and lost as well as RC is lost + * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + + /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +512,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || - (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + + /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -529,7 +534,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost */ + /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost (with datalink enabled) */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; @@ -543,13 +562,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { + /* stay where you are */ + } else if (stay_in_failsafe){ - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else if (!stay_in_failsafe){ - /* everything is perfect */ + /* everything is perfect */ + } else { status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; -- cgit v1.2.3 From f1d56cbddc575115215f680ba42cdedde40b2626 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:41:41 +1000 Subject: navigator: don't reset the finished flag, this fixes the strange problem where it toggles between MISSION and RTL --- src/modules/navigator/navigator_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0da..eff3a5938 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -632,7 +632,6 @@ Navigator::publish_mission_result() } /* reset reached bool */ _mission_result.reached = false; - _mission_result.finished = false; } void -- cgit v1.2.3 From 89724c24a16bd340c33c27531435089e7bd338fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:42:13 +1000 Subject: vehicle_status: whitespace --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e3..91491c148 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -108,7 +108,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ NAVIGATION_STATE_OFFBOARD, NAVIGATION_STATE_MAX, -- cgit v1.2.3 From d15203de91cc07530809060f949374bb8918daea Mon Sep 17 00:00:00 2001 From: mazahner Date: Mon, 10 Nov 2014 17:35:48 +0100 Subject: Debug/NuttX: small changes to make it work with the current stable branch. Debug/NuttX made existing code work with python3 in gdb. Added some new features --- Debug/NuttX | 38 +++-- Debug/Nuttx.py | 433 ++++++++++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 416 insertions(+), 55 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index d34e9f5b4..4b9f4b5a1 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -78,7 +78,7 @@ end ################################################################################ define showfiles - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file) printf "%d files\n", $nfiles set $index = 0 @@ -102,7 +102,7 @@ end ################################################################################ define _showtask_oneline - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf " %p %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name end @@ -139,7 +139,7 @@ end # Print task registers for a NuttX v7em target with FPU enabled. # define _showtaskregs_v7em - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $regs = (uint32_t *)&($task->xcp.regs[0]) printf " r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5] @@ -162,7 +162,7 @@ end define _showsemaphore printf "count %d ", $arg0->semcount if $arg0->holder.htcb != 0 - set $_task = (struct _TCB *)$arg0->holder.htcb + set $_task = (struct tcb_s *)$arg0->holder.htcb printf "held by %s", $_task->name end printf "\n" @@ -172,7 +172,7 @@ end # Print information about a task's stack usage # define showtaskstack - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 if $task == &g_idletcb printf "can't measure idle stack\n" @@ -189,7 +189,7 @@ end # Print details of a task # define showtask - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf "%p %.2d ", $task, $task->pid _showtaskstate $task @@ -204,7 +204,7 @@ define showtask if $task->task_state != TSTATE_TASK_RUNNING _showtaskregs_v7em $task else - _showcurrentregs_v7em + _showtaskregs_v7em $task end # XXX print registers here @@ -247,8 +247,10 @@ define showtasks _showtasklist &g_pendingtasks printf "RUNNABLE\n" _showtasklist &g_readytorun - printf "WAITING\n" + printf "WAITING for Semaphore\n" _showtasklist &g_waitingforsemaphore + printf "WAITING for Signal\n" + _showtasklist &g_waitingforsignal printf "INACTIVE\n" _showtasklist &g_inactivetasks end @@ -257,3 +259,23 @@ document showtasks . showtasks . Print a list of all tasks in the system, separated into their respective queues. end + +define my_mem + + set $start = $arg0 + set $end = $arg1 + set $cursor = $start + + if $start < $end + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor + 4 + printf "0x%x of 0x%x\n",$cursor,$end + end + else + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor - 4 + end + end +end \ No newline at end of file diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index 7cc21b99f..cf86dd668 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -59,30 +59,42 @@ class NX_register_set(object): def __init__(self, xcpt_regs): if xcpt_regs is None: - self.regs['R0'] = long(gdb.parse_and_eval('$r0')) - self.regs['R1'] = long(gdb.parse_and_eval('$r1')) - self.regs['R2'] = long(gdb.parse_and_eval('$r2')) - self.regs['R3'] = long(gdb.parse_and_eval('$r3')) - self.regs['R4'] = long(gdb.parse_and_eval('$r4')) - self.regs['R5'] = long(gdb.parse_and_eval('$r5')) - self.regs['R6'] = long(gdb.parse_and_eval('$r6')) - self.regs['R7'] = long(gdb.parse_and_eval('$r7')) - self.regs['R8'] = long(gdb.parse_and_eval('$r8')) - self.regs['R9'] = long(gdb.parse_and_eval('$r9')) - self.regs['R10'] = long(gdb.parse_and_eval('$r10')) - self.regs['R11'] = long(gdb.parse_and_eval('$r11')) - self.regs['R12'] = long(gdb.parse_and_eval('$r12')) - self.regs['R13'] = long(gdb.parse_and_eval('$r13')) - self.regs['SP'] = long(gdb.parse_and_eval('$sp')) - self.regs['R14'] = long(gdb.parse_and_eval('$r14')) - self.regs['LR'] = long(gdb.parse_and_eval('$lr')) - self.regs['R15'] = long(gdb.parse_and_eval('$r15')) - self.regs['PC'] = long(gdb.parse_and_eval('$pc')) - self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + self.regs['R0'] = self.mon_reg_call('r0') + self.regs['R1'] = self.mon_reg_call('r1') + self.regs['R2'] = self.mon_reg_call('r2') + self.regs['R3'] = self.mon_reg_call('r3') + self.regs['R4'] = self.mon_reg_call('r4') + self.regs['R5'] = self.mon_reg_call('r5') + self.regs['R6'] = self.mon_reg_call('r6') + self.regs['R7'] = self.mon_reg_call('r7') + self.regs['R8'] = self.mon_reg_call('r8') + self.regs['R9'] = self.mon_reg_call('r9') + self.regs['R10'] = self.mon_reg_call('r10') + self.regs['R11'] = self.mon_reg_call('r11') + self.regs['R12'] = self.mon_reg_call('r12') + self.regs['R13'] = self.mon_reg_call('r13') + self.regs['SP'] = self.mon_reg_call('sp') + self.regs['R14'] = self.mon_reg_call('r14') + self.regs['LR'] = self.mon_reg_call('lr') + self.regs['R15'] = self.mon_reg_call('r15') + self.regs['PC'] = self.mon_reg_call('pc') + self.regs['XPSR'] = self.mon_reg_call('xPSR') else: for key in self.v7em_regmap.keys(): self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + def mon_reg_call(self,register): + """ + register is the register as a string e.g. 'pc' + return integer containing the value of the register + """ + str_to_eval = "mon reg "+register + resp = gdb.execute(str_to_eval,to_string = True) + content = resp.split()[-1]; + try: + return int(content,16) + except: + return 0 @classmethod def with_xcpt_regs(cls, xcpt_regs): @@ -172,7 +184,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, stack_limit): + for offset in range(0, int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -187,7 +199,7 @@ class NX_task(object): def state(self): """return the name of the task's current state""" statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) - for name,value in statenames.iteritems(): + for name,value in statenames.items(): if value == self._tcb['task_state']: return name return 'UNKNOWN' @@ -196,16 +208,19 @@ class NX_task(object): def waiting_for(self): """return a description of what the task is waiting for, if it is waiting""" if self._state_is('TSTATE_WAIT_SEM'): - waitsem = self._tcb['waitsem'].dereference() - waitsem_holder = waitsem['holder'] - holder = NX_task.for_tcb(waitsem_holder['htcb']) - if holder is not None: - return '{}({})'.format(waitsem.address, holder.name) - else: - return '{}()'.format(waitsem.address) + try: + waitsem = self._tcb['waitsem'].dereference() + waitsem_holder = waitsem['holder'] + holder = NX_task.for_tcb(waitsem_holder['htcb']) + if holder is not None: + return '{}({})'.format(waitsem.address, holder.name) + else: + return '{}()'.format(waitsem.address) + except: + return 'EXCEPTION' if self._state_is('TSTATE_WAIT_SIG'): return 'signal' - return None + return "" @property def is_waiting(self): @@ -229,7 +244,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = long(filearray[i]['f_inode']) + inode = int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -253,7 +268,18 @@ class NX_task(object): def __str__(self): return "{}:{}".format(self.pid, self.name) - + + def showoff(self): + print("-------") + print(self.pid,end = ", ") + print(self.name,end = ", ") + print(self.state,end = ", ") + print(self.waiting_for,end = ", ") + print(self.stack_used,end = ", ") + print(self._tcb['adj_stack_size'],end = ", ") + print(self.file_descriptors) + print(self.registers) + def __format__(self, format_spec): return format_spec.format( pid = self.pid, @@ -265,7 +291,7 @@ class NX_task(object): file_descriptors = self.file_descriptors, registers = self.registers ) - + class NX_show_task (gdb.Command): """(NuttX) prints information about a task""" @@ -285,7 +311,7 @@ class NX_show_task (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(t, my_fmt) + print(format(t, my_fmt)) class NX_show_tasks (gdb.Command): """(NuttX) prints a list of tasks""" @@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command): def invoke(self, args, from_tty): tasks = NX_task.tasks() + print ('Number of tasks: ' + str(len(tasks))) for t in tasks: - print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + #t.showoff() + print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}')) NX_show_task() NX_show_tasks() @@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command): def __init__(self): super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) - struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') - preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof - if preceding_size == 2: + struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') + preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof + if preceding_size == 2: self._allocflag = 0x8000 - elif preceding_size == 4: + elif preceding_size == 4: self._allocflag = 0x80000000 - else: - raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) - self._allocnodesize = struct_mm_allocnode_s.sizeof + else: + raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) + self._allocnodesize = struct_mm_allocnode_s.sizeof def _node_allocated(self, allocnode): if allocnode['preceding'] & self._allocflag: @@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command): if region_start >= region_end: raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start - print 'heap {} - {}'.format(region_start, region_end) + print ('heap {} - {}'.format(region_start, region_end)) cursor = 1 while cursor < nodecount: allocnode = region_start[cursor] @@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command): state = '' else: state = '(free)' - print ' {} {} {}'.format(allocnode.address + self._allocnodesize, - self._node_size(allocnode), state) + print( ' {} {} {}'.format(allocnode.address + self._allocnodesize, + self._node_size(allocnode), state)) cursor += self._node_size(allocnode) / self._allocnodesize def invoke(self, args, from_tty): @@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print '{} heap(s)'.format(nregions) + print( '{} heap(s)'.format(nregions)) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) @@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(registers, my_fmt) + print (format(registers, my_fmt)) NX_show_interrupted_thread() + +class NX_check_tcb(gdb.Command): + """ check the tcb of a task from a address """ + def __init__(self): + super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + print("tcb int: ",int(args)) + print(tasks[int(args)]._tcb) + a =tasks[int(args)]._tcb['xcp']['regs'] + print("relevant registers:") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) +NX_check_tcb() + +class NX_tcb(object): + def __init__(self): + pass + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + print(li) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + +class NX_check_stack_order(gdb.Command): + """ Check the Stack order corresponding to the tasks """ + + def __init__(self): + super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + return tcb_list + + def getSPfromTask(self,tcb): + regmap = NX_register_set.v7em_regmap + a =tcb['xcp']['regs'] + return int(a[regmap['SP']]) + + def find_closest(self,list,val): + tmp_list = [abs(i-val) for i in list] + tmp_min = min(tmp_list) + idx = tmp_list.index(tmp_min) + return idx,list[idx] + + def find_next_stack(self,address,_dict_in): + add_list = [] + name_list = [] + for key in _dict_in.keys(): + for i in range(3): + if _dict_in[key][i] < address: + add_list.append(_dict_in[key][i]) + if i == 2: # the last one is the processes stack pointer + name_list.append(self.check_name(key)+"_SP") + else: + name_list.append(self.check_name(key)) + + idx,new_address = self.find_closest(add_list,address) + return new_address,name_list[idx] + + def check_name(self,name): + if isinstance(name,(list)): + name = name[0]; + idx = name.find("\\") + newname = name[:idx] + + return newname + + def invoke(self,args,sth): + tcb = self.getTCB(); + stackadresses={}; + for t in tcb: + p = []; + #print(t.name,t._tcb['stack_alloc_ptr']) + p.append(int(t['stack_alloc_ptr'])) + p.append(int(t['adj_stack_ptr'])) + p.append(self.getSPfromTask(t)) + stackadresses[str(t['name'])] = p; + address = int("0x30000000",0) + print("stack address : process") + for i in range(len(stackadresses)*3): + address,name = self.find_next_stack(address,stackadresses) + print(hex(address),": ",name) + +NX_check_stack_order() + +class NX_run_debug_util(gdb.Command): + """ show the registers of a task corresponding to a tcb address""" + def __init__(self): + super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER) + + def printRegisters(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + print("relevant registers in ",task.name,":") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) + + def getPCfromTask(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + return hex(int(a[regmap['PC']])) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + if args == '': + for t in tasks: + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + else: + tcb_nr = int(args); + print("tcb_nr = ",tcb_nr) + t = tasks[tcb_nr] + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + +NX_run_debug_util() + + +class NX_search_tcb(gdb.Command): + """ shot PID's of all running tasks """ + + def __init__(self): + super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def invoke(self,args,sth): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tasks = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tasks = tasks + self.find_tcb_list(cursor) + + # filter for tasks that are listed twice + tasks_filt = {} + for t in tasks: + pid = int(t['pid']); + if not pid in tasks_filt.keys(): + tasks_filt[pid] = t['name']; + print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) + for pid in tasks_filt.keys(): + print("PID: ",pid," ",tasks_filt[pid]) + +NX_search_tcb() + + +class NX_my_bt(gdb.Command): + """ 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list + arg: tcb_address$ + (can easily be found by typing 'showtask'). + """ + + def __init__(self): + super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER) + + def readmem(self,addr): + ''' + read memory at addr and return nr + ''' + str_to_eval = "x/x "+hex(addr) + resp = gdb.execute(str_to_eval,to_string = True) + idx = resp.find('\t') + return int(resp[idx:],16) + + def is_in_bounds(self,val): + lower_bound = int("08004000",16) + upper_bound = int("080ae0c0",16); + #print(lower_bound," ",val," ",upper_bound) + if val>lower_bound and val0: + name = words[3][idx+1:]; + path = words[3][:idx]; + else: + name = words[3]; + path = ""; + block = gdb.block_for_pc(addr) + func = block.function + if str(func) == "None": + func = block.superblock.function + + if valid: + print("Line: ",line," in ",path,"/",name,"in ",func) + return name,path,line,func + + + + + def invoke(self,args,sth): + addr_dec = int(args[2:],16) + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) + up_stack = int(_tcb['adj_stack_ptr']) + curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = int(_tcb['adj_stack_size']) # other stack pointer + + print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) + + if curr_sp == up_stack: + sp = other_sp + else: + sp = curr_sp; + + while(sp < up_stack): + mem = self.readmem(sp) + #print(hex(sp)," : ",hex(mem)) + if self.is_in_bounds(mem): + # this is a potential instruction ptr + stack_percentage = (up_stack-sp)/stacksize + name,path,line,func = self.print_instruction_at(mem,stack_percentage) + sp = sp + 4; # jump up one word + +NX_my_bt() -- cgit v1.2.3 From 88093ebf1af5483d8b5c218adb1ea0c4798ee21e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 18:52:18 +0100 Subject: Hotfix: Fix IO channel mapping --- src/drivers/px4io/px4io.cpp | 47 +++++++++++++++++++++++---------------------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d68d9f60..06faf49a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1245,41 +1245,42 @@ PX4IO::io_set_rc_config() * for compatibility reasons with existing * autopilots / GCS'. */ - param_get(param_find("RC_MAP_ROLL"), &ichan); - - /* subtract one from 1-based index - this might be - * a negative number now - */ - ichan -= 1; - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; + /* ROLL */ + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 0; + } + /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 1; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; - + /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 2; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; - + /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 3; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; - + /* FLAPS */ param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 4; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 4; - + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* -- cgit v1.2.3 From c8a8305326f84cdfd92326b36241caee64b0c529 Mon Sep 17 00:00:00 2001 From: fludwig Date: Wed, 15 Oct 2014 23:48:40 +0200 Subject: added switch to send PWM to Brushless Controller in unarmed state --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 83c470234..0f0cb3a89 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -3,3 +3,6 @@ sh /etc/init.d/rc.fw_defaults set MIXER FMU_Q +# Provide ESC a constant 1000 us pulse while disarmed +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 -- cgit v1.2.3 From e474600978f0fe92de79622a746d06f2c9df77a9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 09:18:31 +1000 Subject: commander: make the failsafe message critical --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f9ea4e914..002a33811 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1852,9 +1852,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; if (status.failsafe) { - mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + mavlink_log_critical(mavlink_fd, "failsafe mode on"); } else { - mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + mavlink_log_critical(mavlink_fd, "failsafe mode off"); } failsafe_old = status.failsafe; } -- cgit v1.2.3 From fa5f3658828280f54a7fec3c33ec3e25e4198561 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:25:53 +1000 Subject: state_machine_helper: fixed comments --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3553dcdc3..e1a626262 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -520,7 +520,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + /* check for RC signal and datalink lost (with datalink enabled) */ } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; @@ -534,7 +534,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + /* check if RC signal is lost (with datalink disabled) after mission is finished */ } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { status->failsafe = true; -- cgit v1.2.3 From 3c9a73f3e41a379e5e7bbf6e98d67facc3b2b49f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:28:24 +1000 Subject: fixed empty if body --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e1a626262..a9a5c4051 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -562,11 +562,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* stay where you are */ - } else if (stay_in_failsafe){ - - /* everything is perfect */ - } else { + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; -- cgit v1.2.3 From d98831c3b50addab0084f9636060cece46ff92bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:42:26 +1000 Subject: navigator: move waypoint reached reset to a more obvious location --- src/modules/navigator/mission.cpp | 1 + src/modules/navigator/navigator_main.cpp | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..b8e3274fd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -707,6 +707,7 @@ Mission::set_mission_item_reached() _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; _navigator->publish_mission_result(); + reset_mission_item_reached(); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index eff3a5938..e933919fb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -630,8 +630,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; } void -- cgit v1.2.3 From 51ffb887c39ecfd0cb94879475f032ee65505321 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:13:08 +0100 Subject: UAVCAN: initialize device id for mag and baro to allow DEVIOCGDEVICEID ioctl to return useful data --- src/drivers/device/device.h | 3 ++- src/modules/uavcan/sensors/mag.cpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 +++ src/modules/uavcan/sensors/sensor_bridge.hpp | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 9d684e394..67aaa0aff 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -130,7 +130,8 @@ public: enum DeviceBusType { DeviceBusType_UNKNOWN = 0, DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2 + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, }; /* diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f..0d9ea08c5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce680..0999938fc 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc..04c59b77d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _channels(new Channel[MaxChannels]) { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; + _device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here } /** -- cgit v1.2.3 From cf2baac516b6f86daf30342be47264b2dcc8abc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 21:45:13 +1000 Subject: navigator: more reset reached calls --- src/modules/navigator/datalinkloss.cpp | 4 ++++ src/modules/navigator/rcloss.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 66f1c8c73..e789fd10d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item() /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -188,6 +189,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { if (!_param_skipcommshold.get()) { @@ -208,6 +210,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; @@ -215,6 +218,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: warnx("dll end"); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 5564a1c42..42392e739 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -163,6 +163,7 @@ RCLoss::advance_rcl() _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); } break; case RCL_STATE_LOITER: @@ -171,6 +172,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: warnx("rcl end"); -- cgit v1.2.3 From 1394b02c2e2158b2796a51ca622c17893f78361f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Nov 2014 13:45:25 +0100 Subject: Make tools executable --- Documentation/versionfilter.sh | 0 Tools/px_generate_xml.sh | 0 Tools/px_update_wiki.sh | 0 3 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 Documentation/versionfilter.sh mode change 100644 => 100755 Tools/px_generate_xml.sh mode change 100644 => 100755 Tools/px_update_wiki.sh diff --git a/Documentation/versionfilter.sh b/Documentation/versionfilter.sh old mode 100644 new mode 100755 diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh old mode 100644 new mode 100755 diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh old mode 100644 new mode 100755 -- cgit v1.2.3 From 50b410664f4087bb4fd50e8bba62267c70719b71 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 17:01:20 +0100 Subject: UAVCAN: set bus number part of device_id to zero --- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 04c59b77d..e31960537 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -113,7 +113,7 @@ protected: { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - _device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here + _device_id.devid_s.bus = 0; } /** -- cgit v1.2.3 From d63c054bb00a0df6666f498950ab797dc07817b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 09:48:32 +1000 Subject: state_machine_helper: changed failsafe behaviour: always require at least one link for default, do RTGS as soon as datalink is lost if datalink loss mode is enabled --- src/modules/commander/state_machine_helper.cpp | 29 ++++++++------------------ 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a9a5c4051..e37019d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if the datalink is enabled and lost as well as RC is lost - * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ /* first look at the commands */ if (status->engine_failure_cmd) { @@ -520,8 +519,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) */ - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -534,8 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with datalink disabled) after mission is finished */ - } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -548,20 +551,6 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost (with datalink enabled) */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; -- cgit v1.2.3 From 20703c8d2352dd137ec13be69e42fe6fdfd9f8f2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:05:06 +1000 Subject: manual_control_setpoint: whitespace --- src/modules/uORB/topics/manual_control_setpoint.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc..c0c33a629 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -47,7 +47,7 @@ * Switch position */ typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_NONE = 0, /**< switch is not mapped */ SWITCH_POS_ON, /**< switch activated (value = 1) */ SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ SWITCH_POS_OFF /**< switch not activated (value = -1) */ @@ -93,12 +93,12 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From b1c6692f2028b616581efaf7a320a7cf7072a02f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:06:25 +1000 Subject: commander: removed unused definition --- src/modules/commander/commander.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd46..149c0f60e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); -- cgit v1.2.3 From f1058bdf0816cda718cee4d5a69c4ac3d53ea781 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:07:27 +1000 Subject: manual_control_setpoint: wrong comment --- src/modules/uORB/topics/manual_control_setpoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c0c33a629..50b7bd9e5 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -99,7 +99,7 @@ struct manual_control_setpoint_s { switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; /**< manual control inputs */ +}; /** * @} -- cgit v1.2.3 From 703fcec62d3430b75876d2c06183bbf4451f0dd6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 12:11:23 +1000 Subject: navigator: fix do jumps: after the repetitions are completed the current mission index is no longer invalid --- src/modules/navigator/mission.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..840c9ec0e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,14 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -630,7 +631,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +640,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } -- cgit v1.2.3 From 08443205e90955c13532c79aa02a283a50cce25d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:07:06 +1000 Subject: navigator: make DO_JUMPS survive power on resets --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 840c9ec0e..cfe85c84e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -627,7 +627,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), -- cgit v1.2.3 From 14b5732c6eff6998d013c0816e983320906c462d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:26:10 +1000 Subject: px4flow: small verbose output fix --- src/drivers/px4flow/px4flow.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 6c68636c6..9e1ecb61e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ @@ -443,7 +443,7 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d", ret); return ret; } -- cgit v1.2.3 From 7bc9a6257731a741ff309f060f6cf87c33c4a266 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 09:52:35 +0100 Subject: code style, warnings --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++------ src/modules/uavcan/uavcan_main.cpp | 9 ++++----- src/systemcmds/motor_test/motor_test.c | 19 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index fbd4f0bcd..1d23099f3 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -113,8 +113,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); - } - else { + } else { msg.cmd.push_back(static_cast(0)); } } @@ -128,18 +127,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) void UavcanEscController::arm_all_escs(bool arm) { - if (arm) + if (arm) { _armed_mask = -1; - else + } else { _armed_mask = 0; + } } void UavcanEscController::arm_single_esc(int num, bool arm) { - if (arm) + if (arm) { _armed_mask = MOTOR_BIT(num); - else + } else { _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 653d4f98c..2c543462e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -346,13 +346,12 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { - float outputs[NUM_ACTUATOR_OUTPUTS] = {}; - outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; + test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; // Output to the bus - _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); - } - else if (controls_updated && (_mixers != nullptr)) { + _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674..087699b05 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -67,19 +67,15 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); } static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } errx(1, "usage:\n" @@ -90,8 +86,9 @@ static void usage(const char *reason) int motor_test_main(int argc, char *argv[]) { - unsigned long channel, lval; - float value; + unsigned long channel = 0; + unsigned long lval; + float value = 0.0f; int ch; if (argc != 5) { @@ -122,7 +119,7 @@ int motor_test_main(int argc, char *argv[]) motor_test(channel, value); - printf("motor %d set to %.2f\n", channel, value); + printf("motor %d set to %.2f\n", channel, (double)value); exit(0); } -- cgit v1.2.3 From 64de403b5f2aa4af57918510451373424c162b68 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 20:06:49 +1000 Subject: navigator: trying to improve a comment --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cfe85c84e..d0611b9e5 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -595,7 +595,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { -- cgit v1.2.3 From ae99a179ac0fd2b7a240d51adf7d992ff91ed598 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 12:21:15 +0100 Subject: Include NuttX sscanf() fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 5ee4b2b2c..47a94a097 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 +Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca -- cgit v1.2.3 From d7ebea6ec2f3783bfdd4becd7cda5fd24c340a85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:01:00 +0100 Subject: Fix comparison in upload script for test builds --- Tools/upload.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/upload.sh b/Tools/upload.sh index 17d87fe61..c0dd65eba 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -10,17 +10,17 @@ SYSTYPE=`uname -s` # # XXX The uploader should be smarter than this. # -if [ $SYSTYPE=Darwin ]; +if [ $SYSTYPE = "Darwin" ]; then SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi -if [ $SYSTYPE=Linux ]; +if [ $SYSTYPE = "Linux" ]; then SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" fi -if [ $SYSTYPE="" ]; +if [ $SYSTYPE = "" ]; then SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" fi -- cgit v1.2.3 From afcbab7fba84cf3d3d016a63fbca405ade15889a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 13:37:58 +0100 Subject: PX4FLOW driver: Prevent sensor from spamming the system on errors. Use the perf system call to look at error counters. --- src/drivers/px4flow/px4flow.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9e1ecb61e..804027b05 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -240,7 +240,7 @@ PX4FLOW::init() _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); if (_px4flow_topic < 0) { - debug("failed to create px4flow object. Did you start uOrb?"); + warnx("failed to create px4flow object. Did you start uOrb?"); } ret = OK; @@ -442,8 +442,6 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d", ret); return ret; } @@ -465,7 +463,7 @@ PX4FLOW::collect() ret = transfer(nullptr, 0, &val[0], 22); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -565,7 +563,7 @@ PX4FLOW::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -592,7 +590,7 @@ PX4FLOW::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + debug("measure error"); } /* next phase is collection */ -- cgit v1.2.3 From 826fd3f4cdf492be1e5b05f7da4493ff10118876 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:43:51 +1000 Subject: commander: reordering navigation states to match enum --- src/modules/commander/commander.cpp | 153 +++++++++++++++++++----------------- 1 file changed, 83 insertions(+), 70 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7917bc9f2..3307c4bfc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2201,18 +2201,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_termination_enabled = false; - break; - case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2225,64 +2213,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_OFFBOARD: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_offboard_enabled = true; - - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - //XXX: the flags could depend on sp_offboard.ignore - break; - - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } - - break; - case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2323,6 +2253,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2336,6 +2279,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; @@ -2349,6 +2305,63 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore + break; + + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + default: break; } -- cgit v1.2.3 From 00cc2d55111fe6dcf03fc847aa6e65204e4a0a2c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:44:17 +1000 Subject: commander: added navstate RCRECOVER which was missing --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3307c4bfc..5c4ad26f4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,9 +2225,11 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: case NAVIGATION_STATE_AUTO_RTGS: case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; -- cgit v1.2.3 From db3f2fc94621ee13c7ec22d5fef1dd5ae74a0742 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:46:12 +1000 Subject: commander: whitespace --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c4ad26f4..ec173c12b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,7 +2225,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: -- cgit v1.2.3 From 6a543b1d0154d7da9ddb4123df1479cf01aa88bc Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 10:45:32 +0100 Subject: build fix --- src/modules/uORB/topics/test_motor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index c880fe099..2dd90e69d 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output power, range [0..1] + float value; /**< output power, range [0..1] */ }; /** -- cgit v1.2.3 From 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:19:15 +0100 Subject: UAVCAN: improve mag compatibility --- src/modules/uavcan/sensors/mag.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 0d9ea08c5..4f56038b5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -74,6 +74,9 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + return OK; // Pretend that this stuff is supported to keep APM happy + } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); return 0; -- cgit v1.2.3 From 0fa622f22b90048ad366ac304c2c9339a5c376d3 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:20:03 +0100 Subject: UAVCAN: declare mag external again to allow different rotation than the internal mag --- src/modules/uavcan/sensors/mag.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f56038b5..4d473291c 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -89,7 +89,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy -- cgit v1.2.3 From 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 13:14:05 +0100 Subject: UACVAN: add read()-style interface to mag device --- src/modules/uavcan/sensors/mag.cpp | 48 +++++++++++++++++++++++++++++--------- src/modules/uavcan/sensors/mag.hpp | 2 ++ 2 files changed, 39 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4d473291c..a529e7e75 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +#include + static const orb_id_t MAG_TOPICS[3] = { ORB_ID(sensor_mag0), ORB_ID(sensor_mag1), @@ -71,6 +73,30 @@ int UavcanMagnetometerBridge::init() return 0; } +ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct mag_report *mag_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct mag_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *mag_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct mag_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -89,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // declare it external rise it's priority and to allow for correct orientation compensation } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy @@ -111,18 +137,18 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::mag_report(); - - report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + lock(); + _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.timestamp = msg.getUtcTimestamp().toUSec(); + if (_report.timestamp == 0) { + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); } - report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + unlock(); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 6d413a8f7..74077d883 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -54,6 +54,7 @@ public: int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; -- cgit v1.2.3 From e026324784409bd16a561544cb608e7089d500a0 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 16:58:41 +0100 Subject: UAVCAN: fix mag report timestamp --- src/modules/uavcan/sensors/mag.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index a529e7e75..5cbb96433 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,7 @@ #include "mag.hpp" +#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -139,11 +140,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Sat, 15 Nov 2014 11:51:21 +0100 Subject: Updated NuttX, only comment changes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 47a94a097..9d06b6457 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca +Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f -- cgit v1.2.3 From 780451dd65db72aa28b75ddad2effbbba8db8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:30 +0100 Subject: Remove unneeded file --- ROMFS/px4fmu_common/init.d/rc.jig | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.jig diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig deleted file mode 100644 index e2b5d8f30..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.jig +++ /dev/null @@ -1,10 +0,0 @@ -#!nsh -# -# Test jig startup script -# - -echo "[testing] doing production test.." - -tests jig - -echo "[testing] testing done" -- cgit v1.2.3 From 4bcb2697415009279bd481eb9d22f015846147f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:49 +0100 Subject: Add capability to read chip ID and revision from ver command --- src/systemcmds/ver/ver.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 84 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9ae080ee2..b11b593c3 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -53,13 +53,38 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +static const char mcu_ver_str[] = "mcu"; + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +/* magic numbers from reference manual */ +enum STM32F4_REV { + STM32F4_REV_A = 0x1000, + STM32F4_REV_Z = 0x1001, + STM32F4_REV_Y = 0x1003, + STM32F4_REV_1 = 0x1007, + STM32F4_REV_3 = 0x2001 +}; +#else +#error stm32 +#endif + static void usage(const char *reason) { if (reason != NULL) { printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -107,6 +132,64 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; +#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + uint32_t abc = getreg32(DBGMCU_IDCODE); + + uint32_t chip_version = abc & DEVID_MASK; + enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; + + printf("CHIP TYPE: "); + + switch (revid) { + case STM32F40x_41x: + printf("STM32F40x"); + break; + case STM32F42x_43x: + printf("STM32F42x"); + break; + default: + printf("STM32F???"); + break; + } + + char rev; + + switch (chip_version) { + + case STM32F4_REV_A: + rev = 'A'; + break; + case STM32F4_REV_Z: + rev = 'Z'; + break; + case STM32F4_REV_Y: + rev = 'Y'; + break; + case STM32F4_REV_1: + rev = '1'; + break; + case STM32F4_REV_3: + rev = '3'; + break; + default: + rev = '?'; + break; + } + + printf("\nHW REV: %c\n", rev); + + if (rev < STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "on USB connectivity combined with\n" + "flash bank #2. This device can only\n" + "utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } + ret = 0; +#endif + } else { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From 8e8dd62fbd9906c980d9e88943a2fda5b90977e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:06 +0100 Subject: Let the uploader print the binary size --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d4e461226..3a4540ac0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up -- cgit v1.2.3 From a7eaf07f4f9d6df54b7602c07ce7fbdf2db700f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:48 +0100 Subject: Motor test: optimize for size --- src/systemcmds/motor_test/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk index eb36d2ded..261428ef9 100644 --- a/src/systemcmds/motor_test/module.mk +++ b/src/systemcmds/motor_test/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = motor_test SRCS = motor_test.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From d4a5f345aabc83506b05277b9dcf71e24700c70d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:38 +0100 Subject: Remove unneeded apps from build --- makefiles/config_px4fmu-v2_default.mk | 17 +---------------- makefiles/config_px4fmu-v2_test.mk | 7 +++++++ 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d17dff5bb..3c65b19e0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,14 +40,8 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl -MODULES += drivers/pca8574 MODULES += drivers/px4flow - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - # # System commands # @@ -61,7 +55,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd @@ -81,10 +74,8 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator # # Vehicle Control @@ -100,12 +91,6 @@ MODULES += modules/mc_pos_control # MODULES += modules/sdlog2 -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - # # Library modules # @@ -139,7 +124,7 @@ MODULES += modules/bottle_drop #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app +#MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a4..6f54b960c 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -49,6 +49,13 @@ MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +# +# Modules to test-build, but not useful for test environment +# +MODULES += modules/attitude_estimator_so3 +MODULES += drivers/pca8574 +MODULES += examples/flow_position_estimator + # # Libraries # -- cgit v1.2.3 From 75bc8136b1d1d773a5664a226b5b766867fe5ff3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:56 +0100 Subject: Build NuttX optimized for size --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/scripts/ld.script | 3 ++- nuttx-configs/px4io-v1/nsh/Make.defs | 2 +- nuttx-configs/px4io-v2/nsh/Make.defs | 6 +----- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 7b2ea703a..e7318e519 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index e70320aaa..f3ce53b4a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index 1be39fb87..bec896d1c 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -50,7 +50,8 @@ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 712631f47..7a0792ff6 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index cd2d8eba3..1717464d2 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -53,15 +53,11 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - # use our linker script LDSCRIPT = ld.script -- cgit v1.2.3 From 3d2a5bae51763e5a542506383c9d97f95fc7d1ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:16 +0100 Subject: Board drivers: Optimize for size --- src/drivers/boards/aerocore/module.mk | 2 ++ src/drivers/boards/px4fmu-v1/module.mk | 2 ++ src/drivers/boards/px4fmu-v2/module.mk | 2 ++ src/drivers/boards/px4io-v1/module.mk | 2 ++ src/drivers/boards/px4io-v2/module.mk | 2 ++ 5 files changed, 10 insertions(+) diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index b53fe0a29..0a2d91009 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -6,3 +6,5 @@ SRCS = aerocore_init.c \ aerocore_pwm_servo.c \ aerocore_spi.c \ aerocore_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b776917..5e1a27d5a 100644 --- a/src/drivers/boards/px4fmu-v1/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk index 99d37eeca..103232b0c 100644 --- a/src/drivers/boards/px4fmu-v2/module.mk +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu2_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15..a7a14dd07 100644 --- a/src/drivers/boards/px4io-v1/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk @@ -4,3 +4,5 @@ SRCS = px4io_init.c \ px4io_pwm_servo.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 85f94e8be..3f0e9a0b3 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -4,3 +4,5 @@ SRCS = px4iov2_init.c \ px4iov2_pwm_servo.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From c0f34dff2605381afacbec2bc1eba6b648daddd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:33 +0100 Subject: STM32 drivers: Optimize for size --- src/drivers/stm32/adc/module.mk | 2 ++ src/drivers/stm32/module.mk | 2 ++ src/drivers/stm32/tone_alarm/module.mk | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk index 48620feea..38ea490a0 100644 --- a/src/drivers/stm32/adc/module.mk +++ b/src/drivers/stm32/adc/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = adc SRCS = adc.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index bb751c7f6..54428e270 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -41,3 +41,5 @@ SRCS = drv_hrt.c \ drv_pwm_servo.c INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk index 827cf30b2..25a194ef6 100644 --- a/src/drivers/stm32/tone_alarm/module.mk +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm SRCS = tone_alarm.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From f7f54062439f0b1c1208d0778f992f7052e1518b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:49 +0100 Subject: FMU driver: optimize for size --- src/drivers/px4fmu/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index a60f1a434..a06323a52 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -8,3 +8,5 @@ SRCS = fmu.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a12c98ba63c12ba08f1fb0f66df7b7079e444e1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:08 +0100 Subject: Sensors: Optimize for size --- src/modules/sensors/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 5b1bc5e86..dfbba92d9 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -42,3 +42,5 @@ SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a869105ba23394cfabb0a0000dd3b7fa3dc9fdb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:34 +0100 Subject: Systemlib: Optimize for size --- src/modules/systemlib/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..5c4f2ec41 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,3 +55,4 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.c +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 54f296ce9d8245845a7a0bf32f5045a44aab40c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:01 +0100 Subject: Sensor drivers: Optimize for size --- src/drivers/hmc5883/module.mk | 2 ++ src/drivers/l3gd20/module.mk | 2 ++ src/drivers/lsm303d/module.mk | 2 ++ src/drivers/mpu6000/module.mk | 2 ++ 4 files changed, 8 insertions(+) diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 5daa01dc5..be2ee7276 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -42,3 +42,5 @@ SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 5630e7aec..3d64d62be 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -8,3 +8,5 @@ SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index b4f3974f4..0421eb113 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -8,3 +8,5 @@ SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 5b4893b12..da9fcc0fc 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -42,3 +42,5 @@ SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 32313a13dd1f98426f83b99310fc22b5adced37c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:16 +0100 Subject: PX4IO driver: Optimize for size --- src/drivers/px4io/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 5b838fb75..924283356 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 42575bbc3763fe2b9e2d3e5b074ec74e4fc57f5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:38 +0100 Subject: Bottle drop: Optimize for size --- src/modules/bottle_drop/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 6b18fff55..960228879 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ bottle_drop_params.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a7bc52754de52ccbf0f958c20ce31408a8b4372e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:54 +0100 Subject: SDLOG2: Optimize for size --- src/modules/sdlog2/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index a28d43e72..d4a00af39 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -43,3 +43,5 @@ SRCS = sdlog2.c \ logbuffer.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 7b962e7d1bb6b60633c116b9d5395b24e6658664 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 14:17:24 +0100 Subject: Version command optimization --- src/systemcmds/ver/ver.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index b11b593c3..9d308bc3e 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -182,9 +182,7 @@ int ver_main(int argc, char *argv[]) if (rev < STM32F4_REV_3) { printf("\n\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" - "on USB connectivity combined with\n" - "flash bank #2. This device can only\n" - "utilize a maximum of 1MB flash safely!\n" + "This device can only utilize a maximum of 1MB flash safely!\n" "http://px4.io/help/errata\n", rev); } ret = 0; -- cgit v1.2.3 From 2a37d274b142f49e250265eb14821d0b4f7e51cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 15:51:55 +0100 Subject: Systemlib: Add MCU version command. ver commandline tool: Add support for MCU version command --- src/modules/systemlib/mcu_version.c | 109 ++++++++++++++++++++++++++++++++++++ src/modules/systemlib/mcu_version.h | 52 +++++++++++++++++ src/modules/systemlib/module.mk | 4 +- src/systemcmds/ver/ver.c | 88 ++++++----------------------- 4 files changed, 179 insertions(+), 74 deletions(-) create mode 100644 src/modules/systemlib/mcu_version.c create mode 100644 src/modules/systemlib/mcu_version.h diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 000000000..4bcf95784 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 + * 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 mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier + * + */ + +#include "mcu_version.h" + +#include + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + switch (revid) { + + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + default: + *rev = '?'; + revid = -1; + break; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 000000000..1b3d0aba9 --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 + * 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. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..233023e25 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,5 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c - + circuit_breaker.c \ + mcu_version.c diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9d308bc3e..62a7a5b92 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -44,6 +44,7 @@ #include #include #include +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; @@ -52,32 +53,8 @@ static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; - -#ifdef CONFIG_ARCH_CHIP_STM32 -#include - static const char mcu_ver_str[] = "mcu"; -#define DBGMCU_IDCODE 0xE0042000 - -#define STM32F40x_41x 0x413 -#define STM32F42x_43x 0x419 - -#define REVID_MASK 0xFFFF0000 -#define DEVID_MASK 0xFFF - -/* magic numbers from reference manual */ -enum STM32F4_REV { - STM32F4_REV_A = 0x1000, - STM32F4_REV_Z = 0x1001, - STM32F4_REV_Y = 0x1003, - STM32F4_REV_1 = 0x1007, - STM32F4_REV_3 = 0x2001 -}; -#else -#error stm32 -#endif - static void usage(const char *reason) { if (reason != NULL) { @@ -132,61 +109,28 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; -#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { - uint32_t abc = getreg32(DBGMCU_IDCODE); - - uint32_t chip_version = abc & DEVID_MASK; - enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; - - printf("CHIP TYPE: "); - - switch (revid) { - case STM32F40x_41x: - printf("STM32F40x"); - break; - case STM32F42x_43x: - printf("STM32F42x"); - break; - default: - printf("STM32F???"); - break; - } char rev; + char* revstr; - switch (chip_version) { - - case STM32F4_REV_A: - rev = 'A'; - break; - case STM32F4_REV_Z: - rev = 'Z'; - break; - case STM32F4_REV_Y: - rev = 'Y'; - break; - case STM32F4_REV_1: - rev = '1'; - break; - case STM32F4_REV_3: - rev = '3'; - break; - default: - rev = '?'; - break; - } + int chip_version = mcu_version(&rev, &revstr); + + if (chip_version < 0) { + printf("UNKNOWN MCU"); + ret = 1; - printf("\nHW REV: %c\n", rev); + } else { + printf("MCU: %s, rev. %c\n", revstr, rev); - if (rev < STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" - "Revision %c has a silicon errata\n" - "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); + if (chip_version < MCU_REV_STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "This device can only utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } } - ret = 0; -#endif } else { errx(1, "unknown command.\n"); -- cgit v1.2.3 From 6d59df1a5f67a58b6881d4d8a466082b847a2e61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:19:51 +0100 Subject: Make ROMFS less chatty --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 8 ++------ ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 3 +-- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 4 +--- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 4 ++-- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 4 +--- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 6 +----- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/3035_viper | 2 +- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 8 ++------ ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4008_ardrone | 3 --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 2 +- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 2 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 ++-- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 4 ++-- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++--- ROMFS/px4fmu_common/init.d/rc.sensors | 7 +------ ROMFS/px4fmu_common/init.d/rcS | 24 +++------------------- 30 files changed, 39 insertions(+), 83 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index d114fe21a..4d6d350b8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,10 @@ #!nsh # -# HILStar / X-Plane -# -# Lorenz Meier +# HILStar +# # sh /etc/init.d/rc.fw_defaults -echo "X Plane HIL starting.." - set HIL yes - set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b1aa8c00b..c8379e3a1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks , Thomas Gubler +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 3f47390c1..0b422de7e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 6179855f6..a621de7ce 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -2,8 +2,7 @@ # # Steadidrone QU4D # -# Thomas Gubler -# Lorenz Meier +# Thomas Gubler # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 7a7a9542c..1c4f6803b 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c47500c7a..0cbdd75be 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 4e3e18326..fb440d2fc 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,13 +2,11 @@ # # HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." - set HIL yes set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 941f5664a..00b97d675 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index daa04a4de..e4d96fbd5 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,12 +2,12 @@ # # Generic 10" Hexa coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8703f5f2f..f820251ad 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10" Octo coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 53c48d8aa..a7749cba6 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Simon Wilks , Thomas Gubler +# Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -21,8 +21,6 @@ then param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 param set FW_RR_I 0.02 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 7d0dc5bff..26c7c95e6 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults @@ -19,10 +19,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index f4dedef15..919eefb4a 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index f4bd18269..4a76ba6eb 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index f3b0e8418..0f5f5502a 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -2,7 +2,7 @@ # # Viper # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9a2150403..9bfd9d9ed 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,8 +1,8 @@ #!nsh # -# TBS Caipirinha Flying Wing +# TBS Caipirinha # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults @@ -22,10 +22,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 06c54a41d..8fe8961c5 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10" Quad X geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index e6007db0e..0488e3928 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,9 +3,6 @@ # ARDrone # -echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" - -# Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults # diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 282ab620d..f0cc05207 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F330 Quadcopter +# DJI Flame Wheel F330 # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 517b4aa86..1ca716a6b 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F450 Quadcopter +# DJI Flame Wheel F450 # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 8c5a4fbf2..5c4a6497a 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -2,7 +2,7 @@ # # F450-sized quadrotor with CAN # -# Lorenz Meier +# Pavel Kirienko # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 99ffd73a5..9fe310dde 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -3,7 +3,7 @@ # Hobbyking Micro Integrated PCB Quadcopter # with SimonK ESC firmware and Mystery A1510 motors # -# Thomas Gubler +# Thomas Gubler # echo "HK Micro PCB Quad" diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 1fb25e5d8..5512aa738 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10" Quad + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 34fc6523f..1043ad8ad 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,12 +2,12 @@ # # Generic 10" Hexa X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 235e376a6..84ab88883 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,12 +2,12 @@ # # Generic 10" Hexa + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 769217dc7..74e304cd9 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10" Octo X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 28b074a54..f7c06c6c8 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10" Octo + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 78778d806..496a52c5f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -1,5 +1,4 @@ # -# Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # # AUTOSTART PARTITION: @@ -18,7 +17,7 @@ # 12000 .. 12999 Octo Cox # -# Simulation setups +# Simulation # if param compare SYS_AUTOSTART 901 @@ -53,7 +52,7 @@ then fi # -# Standard plane +# Plane # if param compare SYS_AUTOSTART 2100 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 739df7ac0..fbac50cf7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -54,7 +54,6 @@ then fi fi -# Start airspeed sensors if meas_airspeed start then echo "[init] Using MEAS airspeed sensor" @@ -68,16 +67,12 @@ else fi fi -# Check for flow sensor if px4flow start then fi # -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. +# Start sensors -> preflight_check # if sensors start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea04ece34..f9c822635 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -137,9 +137,7 @@ then # if param compare SYS_AUTOCONFIG 1 then - # We can't be sure the defaults haven't changed, so - # if someone requests a re-configuration, we do it - # cleanly from scratch (except autostart / autoconfig) + # Wipe out params param reset_nostart set DO_AUTOCONFIG yes else @@ -202,12 +200,10 @@ then if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else - echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB @@ -217,18 +213,15 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi @@ -281,16 +274,12 @@ then fi fi - # - # Start the datamanager (and do not abort boot if it fails) - # + # waypoint storage if dataman start then fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # + # Needs to be this early for in-air-restarts commander start # @@ -424,9 +413,6 @@ then fi fi - # - # MAVLink - # if [ $MAVLINK_FLAGS == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s @@ -454,10 +440,6 @@ then # Sensors, Logging, GPS # sh /etc/init.d/rc.sensors - - # - # Start logging in all modes, including HIL - # sh /etc/init.d/rc.logging if [ $GPS == yes ] -- cgit v1.2.3 From e2c0ac3f700f003061e52fd61fd4770c982605c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:29 +0100 Subject: airspeed driver: Be less chatty --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 293690d27..3a1e1b7b5 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -147,7 +147,7 @@ Airspeed::init() _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. uORB started?"); + warnx("uORB started?"); } ret = OK; -- cgit v1.2.3 From 6f71173f8c7a13565ef90a8fec44187ba392f038 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:42 +0100 Subject: ARDrone driver: be less chatty --- src/drivers/ardrone_interface/ardrone_interface.c | 36 +++++++++------------- .../ardrone_interface/ardrone_motor_control.c | 2 +- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index e5bb772b3..9d2c1441d 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -89,8 +89,8 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); + warnx("%s\n", reason); + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("ardrone_interface already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tardrone_interface is running\n"); + warnx("running"); } else { - printf("\tardrone_interface not started\n"); + warnx("not started"); } exit(0); } @@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: tcsetattr: %s", uart_name); close(uart); return -1; } @@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) char *device = "/dev/ttyS1"; - /* welcome user */ - printf("[ardrone_interface] Control started, taking over motors\n"); - /* File descriptors */ int gpios; @@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) struct termios uart_config_original; if (motor_test_mode) { - printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + warnx("setting 10 %% thrust.\n"); } /* Led animation */ @@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - printf("[ardrone_interface] Motors initialized - ready.\n"); - fflush(stdout); - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); @@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("No UART, exiting."); thread_running = false; exit(ERROR); } @@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("write fail"); thread_running = false; exit(ERROR); } @@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int termios_state; if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); + warnx("ERR: tcsetattr"); } - printf("[ardrone_interface] Restored original UART config, exiting..\n"); - /* close uarts */ close(ardrone_write); ar_multiplexing_deinit(gpios); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index fc017dd58..4fa24275f 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios) ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); if (errcounter != 0) { - fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); + warnx("Failed %d times", -errcounter); fflush(stdout); } return errcounter; -- cgit v1.2.3 From 8d5225b9679c2820a57ea30ed6d4c46f5d6c9baa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:18 +0100 Subject: FrSky: Be less chatty --- src/drivers/frsky_telemetry/frsky_telemetry.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6e0839043..bccdf1190 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); return -1; } @@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static const speed_t speed = B9600; if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; } @@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - /* Print welcome text */ - warnx("FrSky telemetry interface starting..."); - /* Open UART */ struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); -- cgit v1.2.3 From ef27225534df6cca0b5a98b85c6b7cf3364493c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:30 +0100 Subject: HIL: Be less chatty --- src/drivers/hil/hil.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f0dc0c651..9b5c8133b 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -442,8 +442,6 @@ HIL::task_main() /* make sure servos are off */ // up_pwm_servo_deinit(); - log("stopping"); - /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ -- cgit v1.2.3 From 5af710221a2948ab14f95225b4cb6c7fe5c66560 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:34 +0100 Subject: HoTT: Less chatty --- src/drivers/hott/comms.cpp | 8 ++++---- src/drivers/hott/hott_sensors/hott_sensors.cpp | 6 +++--- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index cb8bbba37..60a49b559 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -55,7 +55,7 @@ open_uart(const char *device) const int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - err(1, "Error opening port: %s", device); + err(1, "ERR: opening %s", device); } /* Back up the original uart configuration to restore it after exit */ @@ -63,7 +63,7 @@ open_uart(const char *device) struct termios uart_config_original; if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + err(1, "ERR: %s: %d", device, termios_state); } /* Fill the struct for the new configuration */ @@ -76,13 +76,13 @@ open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + err(1, "ERR: %s (tcsetattr)", device); } /* Activate single wire mode */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index a3d3a3933..8ab9d8d55 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index d293f9954..edbb14172 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); -- cgit v1.2.3 From 1fc7b588945b3bb1df70de1f779201c60a803b71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:47 +0100 Subject: Bottle drop: Less chatty --- src/modules/bottle_drop/bottle_drop.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 31c9157e1..6d24e5d2d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -283,7 +283,6 @@ BottleDrop::drop() // force the door open if we have to if (_doors_opened == 0) { open_bay(); - warnx("bay not ready, forced open"); } while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { @@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { open_bay(); drop(); - mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + mavlink_log_critical(_mavlink_fd, "drop bottle"); } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + mavlink_log_critical(_mavlink_fd, "opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + mavlink_log_critical(_mavlink_fd, "closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; - mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + mavlink_log_critical(_mavlink_fd, "got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + mavlink_log_critical(_mavlink_fd, "got drop position and approval"); break; default: @@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; default: -- cgit v1.2.3 From beee7a89ed6e7a7c98ee712ab571c58eb6f3a692 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:04 +0100 Subject: Airspeed: less chatty --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1d9a463ad..ba46de379 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -519,7 +519,7 @@ test() ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + errx(1, "timed out"); } /* now go get it */ -- cgit v1.2.3 From d602c9a0c5eff39e0f533411b45f631cae63e486 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:18 +0100 Subject: Controllib: Optimize for size --- src/modules/controllib/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index f0139a4b7..2860d1efc 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -39,3 +39,5 @@ SRCS = test_params.c \ block/BlockParam.cpp \ uorb/blocks.cpp \ blocks.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 9c7503ba7a3f5d8f98ab3017d221192489d0b3c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 18:38:33 +0100 Subject: uORB: Save space, does not do complex operations --- src/modules/uORB/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 0c29101fe..9385ce253 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -44,3 +44,5 @@ SRCS = uORB.cpp \ objects_common.cpp \ Publication.cpp \ Subscription.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 488739cc467cea08374e97f96f70317376ec58ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:13:46 +0100 Subject: Fix up ver command handling to print MCU version on all command as well --- src/systemcmds/ver/ver.c | 58 ++++++++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 24 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 62a7a5b92..eebeb9289 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -46,7 +46,7 @@ #include #include -// string constants for version commands +/* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; @@ -68,49 +68,56 @@ __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) { - int ret = 1; //defaults to an error + /* defaults to an error */ + int ret = 1; - // first check if there are at least 2 params + /* first check if there are at least 2 params */ if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - printf("%s\n", HW_ARCH); - ret = 0; - } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { - // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + /* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */ ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); + return ret; } } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } + } - } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("%s\n", FW_GIT); - ret = 0; + /* check if we want to show all */ + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) - } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - printf("%s %s\n", __DATE__, __TIME__); + if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { + printf("HW arch: %s\n", HW_ARCH); ret = 0; - } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - printf("%s\n", __VERSION__); + } + + if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("FW git-hash: %s\n", FW_GIT); ret = 0; - } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { - printf("HW arch: %s\n", HW_ARCH); + } + + if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { printf("Build datetime: %s %s\n", __DATE__, __TIME__); - printf("FW git-hash: %s\n", FW_GIT); - printf("GCC toolchain: %s\n", __VERSION__); ret = 0; + } - } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("Toolchain: %s\n", __VERSION__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { char rev; char* revstr; @@ -118,21 +125,24 @@ int ver_main(int argc, char *argv[]) int chip_version = mcu_version(&rev, &revstr); if (chip_version < 0) { - printf("UNKNOWN MCU"); + printf("UNKNOWN MCU\n"); ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); if (chip_version < MCU_REV_STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" + printf("\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); + "http://px4.io/help/errata\n\n", rev); } } - } else { + ret = 0; + } + + if (ret = 1) { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From ed0f28ed4191c296aa3592088dc7875a8246cabb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:43:11 +0100 Subject: Fix ver command --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index eebeb9289..55cec149f 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -91,7 +91,7 @@ int ver_main(int argc, char *argv[]) } /* check if we want to show all */ - bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { printf("HW arch: %s\n", HW_ARCH); -- cgit v1.2.3 From e2551d491754b752b8b835a302acaf3f4cf1f9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:05:54 +0100 Subject: Fixed typo in ver command --- src/systemcmds/ver/ver.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 55cec149f..c794f5819 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -126,7 +126,6 @@ int ver_main(int argc, char *argv[]) if (chip_version < 0) { printf("UNKNOWN MCU\n"); - ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); @@ -142,7 +141,7 @@ int ver_main(int argc, char *argv[]) ret = 0; } - if (ret = 1) { + if (ret == 1) { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From 18dc5e342905f6171b54adba6de8a27d65d62ab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:07:32 +0100 Subject: Fix compile warning in MAVLink app --- src/modules/mavlink/mavlink_messages.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b4911427f..bed915bbe 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); } -- cgit v1.2.3 From c0f32d44a2acd0f840b4246f90816a95dc6a7527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:15:40 +0100 Subject: First stab at animation --- src/modules/px4iofirmware/px4io.c | 40 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 30f32b38e..51281f660 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,7 +125,45 @@ heartbeat_blink(void) static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); #ifdef GPIO_LED4 - LED_RING(heartbeat); + + const unsigned max_brightness = 1000; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + + LED_RING(on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness_counter = 0; + on_counter = 0; + } + #endif } -- cgit v1.2.3 From 63707ef058f01e324d8d74cf6bc48635e09ab507 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:57:23 +0100 Subject: Let it breathe --- src/modules/px4iofirmware/px4io.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 51281f660..8abfb0d2e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in; #define NUM_MSG 2 static char msg[NUM_MSG][40]; +static void heartbeat_blink(void); +static void ring_blink(void); + /* * add a debug message to be printed on the console */ @@ -124,9 +127,16 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +} + +static void +ring_blink(void) +{ #ifdef GPIO_LED4 - const unsigned max_brightness = 1000; + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; static unsigned counter = 0; static unsigned brightness = max_brightness; @@ -135,7 +145,7 @@ heartbeat_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); LED_RING(on); brightness_counter++; @@ -159,9 +169,10 @@ heartbeat_blink(void) n = 62 - counter; } - brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness = (n * n) / 9; brightness_counter = 0; on_counter = 0; + counter++; } #endif @@ -338,6 +349,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + ring_blink(); + check_reboot(); /* check for debug activity (default: none) */ -- cgit v1.2.3 From 8583cf4dcc0e88842cdf717de26a4f0daa63729a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:59:18 +0100 Subject: Let it breathe stronger --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 8abfb0d2e..349aaac57 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -169,7 +169,7 @@ ring_blink(void) n = 62 - counter; } - brightness = (n * n) / 9; + brightness = (n * n) / 8; brightness_counter = 0; on_counter = 0; counter++; -- cgit v1.2.3 From 8caada2188b9c9a8fcab70fbad92ab14506c2427 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 01:21:47 +0100 Subject: Breathe animation, led solid on arming --- src/modules/px4iofirmware/px4io.c | 14 ++++++++++++-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 349aaac57..14ee9cb40 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -134,6 +134,12 @@ ring_blink(void) { #ifdef GPIO_LED4 + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + // XXX this led code does have // intentionally a few magic numbers. const unsigned max_brightness = 118; @@ -147,7 +153,11 @@ ring_blink(void) bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); - LED_RING(on); + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); brightness_counter++; if (on) { @@ -349,7 +359,7 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } - ring_blink(); + ring_blink(); check_reboot(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8186e4c78..93a33490f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -- cgit v1.2.3 From 4e8387465e01cf16e0fbbe94b5e9a3c168540b77 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Nov 2014 13:38:41 +1000 Subject: navigator: only update sensor_combined topic with 50Hz --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e933919fb..ed9e999a9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -289,8 +289,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; -- cgit v1.2.3 From 4d69314a9925be8ded1b9cb723bd01c21f01a816 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 10:55:18 +0100 Subject: Adjust navigator priority --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ed9e999a9..3ca6ac2c0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -498,7 +498,7 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT + 20, 2000, (main_t)&Navigator::task_main_trampoline, nullptr); -- cgit v1.2.3 From 88bae21ce53fc6acadaae4c8271f3d0fce18a721 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters --- src/drivers/drv_pwm_output.h | 26 ++++++++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6873f24b6..b41f088eb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -117,6 +117,23 @@ struct pwm_output_values { unsigned channel_count; }; + +/** + * RC config values for a channel + * + * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a + * param_get() dependency + */ +struct pwm_output_rc_config { + uint8_t channel; + uint16_t rc_min; + uint16_t rc_trim; + uint16_t rc_max; + uint16_t rc_dz; + uint16_t rc_assignment; + bool rc_reverse; +}; + /* * ORB tag for PWM outputs. */ @@ -216,6 +233,15 @@ ORB_DECLARE(output_pwm); /** force safety switch on (to enable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) + +/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) + +/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 06faf49a4..dc33f7657 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2572,6 +2572,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_RC_CONFIG: { + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; + if (config->channel >= _max_actuators) { + /* fail with error */ + return E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; + } + + case PWM_SERVO_SET_OVERRIDE_OK: + /* set the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK); + break; + + case PWM_SERVO_CLEAR_OVERRIDE_OK: + /* clear the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); -- cgit v1.2.3 From 6406e235d6f5cb37a914442036111f616b4b3839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Nov 2014 10:32:21 +1100 Subject: px4io: publish pwm values when STATUS_FLAGS_FMU_OK is not set this allows testing of FMU failure behaviour in px4io by monitoring the reported PWM output when the vehicle code stops sending updates. Otherwise testing needs to be done with "px4io status" which is very tedious. With this change a GCS can monitor the PWM outputs from the failsafe mixer using normal mavlink messages --- src/drivers/px4io/px4io.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dc33f7657..6a313b322 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1657,10 +1657,6 @@ PX4IO::io_publish_raw_rc() int PX4IO::io_publish_pwm_outputs() { - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From ba811254536b6a080d4b3379106bf0e648a51eb0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:03 +1100 Subject: px4io: added OVERRIDE_IMMEDIATE arming flag this allows the flight code to choose whether FMU failure gives immediate manual pilot control, or waits for the mode switch to go past the override threshold --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 18 ++++++++++++++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 3 ++- 5 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index b41f088eb..edb72f04e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -242,6 +242,9 @@ ORB_DECLARE(output_pwm); /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ #define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a313b322..519ba663a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2057,7 +2057,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2067,7 +2067,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2307,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is + immediate or waits for override threshold on mode + switch */ + if (arg == 0) { + /* clear override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { + /* set override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e..e3cb8d820 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -417,6 +417,15 @@ controls_tick() { if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 9b2e047cb..c7e9ae3eb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,7 @@ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 49c2a9f56..fbfdd35db 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) -- cgit v1.2.3 From f533c362515e31b536c34d83342c396e54a7d652 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters Conflicts: src/drivers/drv_pwm_output.h --- src/drivers/drv_pwm_output.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index edb72f04e..b10c3e18a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -231,16 +231,16 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ -#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) -- cgit v1.2.3 From 852fa611730be2433156b010c6162606ec857ef3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Nov 2014 14:12:58 -0800 Subject: Correct setting of DSM and ST24 flags --- src/modules/px4iofirmware/controls.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3fd73fc60..75d80ba5d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -71,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -172,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); -- cgit v1.2.3 From 52d5a7c00aa74fe0d4241f7e6636dfd6e48d0acf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 23:24:52 +0100 Subject: Fix PWM command --- src/systemcmds/pwm/pwm.c | 68 ++++++++++++++++++++++++------------------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 478c2a772..eeba89fa8 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -74,34 +74,34 @@ usage(const char *reason) "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - " arm Arm output\n" - " disarm Disarm output\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" "\n" - " rate ... Configure PWM rates\n" - " [-g ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply channel mask\n" - " [-a] Configure all outputs\n" - " -r PWM rate (50 to 400 Hz)\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM rate (50 to 400 Hz)\n" "\n" - " failsafe ... Configure failsafe PWM values\n" - " disarmed ... Configure disarmed PWM values\n" - " min ... Configure minimum PWM values\n" - " max ... Configure maximum PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " test ... Directly set PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " info Print information about the PWM device\n" + "info\t\t\t\tPrint information\n" "\n" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 1) + if (argc < 2) usage(NULL); while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { @@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[]) /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("BAD set_mask VAL"); break; case 'a': @@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[]) case 'p': pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad PWM value provided"); + usage("BAD PWM VAL"); break; case 'r': alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alternative rate provided"); + usage("BAD rate VAL"); break; default: break; @@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[]) } if (print_verbose && set_mask > 0) { - warnx("Chose channels: "); + warnx("Channels: "); printf(" "); for (unsigned i = 0; i Date: Mon, 17 Nov 2014 14:07:13 +0100 Subject: Fix output of ver hwcmd call --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index c794f5819..2ead3e632 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -82,8 +82,8 @@ int ver_main(int argc, char *argv[]) if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); - return ret; } + return ret; } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); -- cgit v1.2.3 From f20f85f0e3c61ce754fd6d9d73745eaa925b3b04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:07:33 +0100 Subject: Do not spam filter resets in static mode --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 685f5e12f..00900c995 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state() warn_index = max_warn_index; } - warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + // Do not warn about accel offset if we have no position updates + if (!(warn_index == 5 && _ekf->staticMode)) { + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + } } struct estimator_status_report rep; -- cgit v1.2.3 From 41fe04776f362cf9f0fae6451550df8d5eaf8d99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:59:36 +0100 Subject: Fix up stack sizes --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/bottle_drop/module.mk | 2 ++ src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 6d24e5d2d..e0bcbc6e9 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -223,7 +223,7 @@ BottleDrop::start() _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, - 2048, + 1500, (main_t)&BottleDrop::task_main_trampoline, nullptr); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 960228879..df9f5473b 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -41,3 +41,5 @@ SRCS = bottle_drop.cpp \ bottle_drop_params.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 00900c995..e7805daa9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1560,7 +1560,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 5000, + 7500, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c251dd3b2..fb9f65cf5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2900, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3ca6ac2c0..10a4ee88f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -499,7 +499,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 2000, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); -- cgit v1.2.3 From 97a1410ec99e880207e4ee6d2a03451c2e11f4cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:03:50 +0100 Subject: Toolchain: Allow GCC 4.7 and 4.8 variants --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 71c7fb49f..84e5cd08a 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif -- cgit v1.2.3 From 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Mon, 17 Nov 2014 19:58:18 -0500 Subject: fixed yaw/yawrate bit masking. fixed navigator overriding offboard setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9..c0a8d7322 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,10 +540,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - OFB_IGN_BIT_YAW; + (OFB_IGN_BIT_YAW - 10); offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - OFB_IGN_BIT_YAWRATE; + (OFB_IGN_BIT_YAWRATE - 11); offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10a4ee88f..5e9806499 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -468,9 +468,10 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + static bool published_once = false; + if (_navigation_mode == nullptr && !published_once) { + published_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; -- cgit v1.2.3 From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:26:29 -0500 Subject: improved readability of offboard fixes --- src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++---- src/modules/navigator/navigator.h | 1 + src/modules/navigator/navigator_main.cpp | 6 +++--- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0a8d7322..ca00d1a67 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - (OFB_IGN_BIT_YAW - 10); + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - (OFB_IGN_BIT_YAWRATE - 11); + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d550dcc4c..9cd609955 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -205,6 +205,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5e9806499..035b0d18c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,6 +137,7 @@ Navigator::Navigator() : _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -469,9 +470,8 @@ Navigator::task_main() } /* if nothing is running, set position setpoint triplet invalid once */ - static bool published_once = false; - if (_navigation_mode == nullptr && !published_once) { - published_once = true; + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; -- cgit v1.2.3 From 69271a7251a0bcfd980c3534194b73fc45fa5f23 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:40:52 -0500 Subject: made invalid setpoints publish once every time it enters an invalid state --- src/modules/navigator/navigator_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 035b0d18c..df620e5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -428,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -441,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -453,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: -- cgit v1.2.3 From 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 18 Nov 2014 14:58:04 +0100 Subject: UAVCAN: preserve original UAVCAN message timestamps --- src/modules/uavcan/sensors/baro.cpp | 6 +----- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 -- src/modules/uavcan/sensors/mag.cpp | 3 +-- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 80c5e3828..8741ae20d 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< { auto report = ::baro_report(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - + report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 24afe6aaf..a375db37f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure - #include #include diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 5cbb96433..35ebee542 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,7 +37,6 @@ #include "mag.hpp" -#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Tue, 18 Nov 2014 17:20:50 -0800 Subject: Parameter xml metadata in .px4 --- Tools/px_mkfw.py | 5 +++++ makefiles/config_px4fmu-v1_default.mk | 3 +++ makefiles/config_px4fmu-v2_default.mk | 3 +++ makefiles/firmware.mk | 9 +++++++++ 4 files changed, 20 insertions(+) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index b598a65a1..c2da8a203 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string") parser.add_argument("--summary", action="store", help="set a brief description") parser.add_argument("--description", action="store", help="set a longer description") parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file") parser.add_argument("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -101,6 +102,10 @@ if args.git_identity != None: p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") bytes = f.read() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9fe16fbb6..4507b506c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -132,6 +132,9 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3c65b19e0..d3b8ee93e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,9 @@ MODULES += modules/bottle_drop # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 60602e76f..21e8739aa 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -467,6 +467,7 @@ endif PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ --image $< > $@ +else + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --image $< > $@ +endif $(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) -- cgit v1.2.3 From 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 20 Nov 2014 17:25:30 +0100 Subject: commander: Change printing in RC-loss message to integers --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d8ebee4b6..1f13aaec4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; -- cgit v1.2.3 From 9bb0ecf0ca6082355072af190018e0b5298b7e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:06:32 +0100 Subject: Airspeed calibration feedback: Improve wording --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e37019d02..465f9cdc5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } } -- cgit v1.2.3 From 6da9063560b900c0ce13bb0e078889eeaf8c71c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12b..e081955d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } -- cgit v1.2.3 From 366c8a9c41e22ff31a8e027c5f931b99b7569a7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953..41e0b149b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c822635..201b99749 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" -- cgit v1.2.3 From b2671c8f058fae56c468b68d3185bfc1e8626710 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8..47c907bd3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); -- cgit v1.2.3 From 7bed194f4a217835b78f89c9e8399e1c946ca579 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: EKF: less verbose --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 35dc39ec6..e2dbc30dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ -- cgit v1.2.3 From 2fbda61521131f66cd3aab792716f3c40bc1cac8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c..81a13edb8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From df37f380cb792570ae07b1cc3e9eb61e3495fc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..e3c94bc46 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions -- cgit v1.2.3 From 90f28647539e7c8ae36ac74c7526582f3f18b3e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: INAV: Less verbose --- .../position_estimator_inav/position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..f82a0f989 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel -- cgit v1.2.3 From 1709c74f82da510fff5bf546a75c9894a19d8fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d8..705e54be3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ -- cgit v1.2.3 From b3542bec085a598f34c2e796ee5c78328d0d990c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e3c94bc46..5918d6bc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f82a0f989..e736a86d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -387,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -518,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -719,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } -- cgit v1.2.3 From 828163f2f5c6b68d9f262b1dd4f57e641c1d45a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:09 +0100 Subject: Update mixing handling to allow IO safety off updates --- src/modules/px4iofirmware/mixer.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c0b8ac358..66f0969de 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0; int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while safety off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } - /* abort if we're in the mixer */ + /* disable mixing, will be enabled once load is complete */ + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + + /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } @@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); - if (length < sizeof(px4io_mixdata)) + if (length < sizeof(px4io_mixdata)) { return 0; + } - unsigned text_length = length - sizeof(px4io_mixdata); + unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; @@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* disable mixing during the update */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; -- cgit v1.2.3 From 300705321a0ffa79ab3d4d53e3c029fe8238086a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:59 +0100 Subject: Allow IO safety off system handling as long as the total system is not live --- src/modules/px4iofirmware/registers.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fbfdd35db..a1a02965f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -407,10 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); break; default: @@ -583,8 +584,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + // do not reboot if FMU is armed and IO's safety is off + // this state defines an active system. + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { // don't allow reboot while armed break; } @@ -631,9 +634,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /** * do not allow a RC config change while outputs armed + * = FMU is armed and IO's safety is off + * this state defines an active system. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } -- cgit v1.2.3 From a36088b9c20e52e37d48e7a08aca39d5f8b901f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..769f84cb6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..c46d22733 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -520,7 +520,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -721,8 +721,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } -- cgit v1.2.3 From 3c5c1d3c89639d3e67086386d23d10e0dc8f65b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12b..e081955d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } -- cgit v1.2.3 From 00961b55928a949a2dd3b7022fe2cbb4df06ec22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953..41e0b149b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c822635..201b99749 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" -- cgit v1.2.3 From 54e7ed70e135f20a3470ebc71dae700e8c223102 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8..47c907bd3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); -- cgit v1.2.3 From b34b40622ba7bbd08e7ca5bc0571612ba96fc7ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: EKF: less verbose --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 35dc39ec6..e2dbc30dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ -- cgit v1.2.3 From 2a76b10f7a76273ac3b16d5ac917d40ea28a277b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c..81a13edb8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From 4c281030bb782f852600daf217d7a75b23489bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 769f84cb6..5918d6bc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions -- cgit v1.2.3 From ace6c3fe409f4b70e75bb762e0ba6e0a574a7277 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: INAV: Less verbose --- .../position_estimator_inav/position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c46d22733..e736a86d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel -- cgit v1.2.3 From 138c25ec74f08db6f6759053d0200058a794c196 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d8..705e54be3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ -- cgit v1.2.3 From 512779907e06f059a15d54c88d71b73aad9aced0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 13:01:00 +0100 Subject: Update NuttX version, MD5 fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 9d06b6457..ec6b670f6 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f +Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097 -- cgit v1.2.3 From 2f271888d2ed934c271637c22554b503ce68e535 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 19:22:55 +0100 Subject: Added performance counter for SD log performance of write() call --- src/modules/sdlog2/sdlog2.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f7..8638a4904 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } -- cgit v1.2.3 From 08d6cbe6bf0b5b04f63e42c6c60f5b1fe6167547 Mon Sep 17 00:00:00 2001 From: philipoe Date: Sun, 23 Nov 2014 21:23:01 +0100 Subject: commander: Decrease RC-signal-regained message length to stay within 50 character length limit at all times --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index deb048566..f77895efb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1544,7 +1544,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } -- cgit v1.2.3 From 2dae1bc542746ed373458b55a8e564607700d17d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:05:10 +1100 Subject: uavcan: break the link between poll fd indexes and controls this linkage was fragile and makes it harder to add new orb subscriptions to the uavcan code --- src/modules/uavcan/uavcan_main.cpp | 5 ++--- src/modules/uavcan/uavcan_main.hpp | 3 +++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2c543462e..ccc087920 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -333,14 +333,12 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { + if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - poll_id++; } } @@ -474,6 +472,7 @@ UavcanNode::subscribe() if (_control_subs[i] > 0) { _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; + _poll_ids[i] = _poll_fds_num; _poll_fds_num++; } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 274321f0d..b791663d4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -127,4 +127,7 @@ private: orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + + // index into _poll_fds for each _control_subs handle + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From f6b0a3e07f27f34af6e6d209aec761374611e968 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:35:58 +1100 Subject: uORB: added actuator_direct topic this topic will be used to allow direct output of actuator values for uavcan, bypassing the mixer. --- src/modules/uORB/Publication.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/actuator_direct.h | 69 +++++++++++++++++++++++++++++++ 3 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/actuator_direct.h diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd6..71757e1f4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -46,6 +46,7 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" +#include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" @@ -76,6 +77,7 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b91a00c1e..49dfc7834 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/actuator_direct.h" +ORB_DEFINE(actuator_direct, struct actuator_direct_s); + #include "topics/multirotor_motor_limits.h" ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h new file mode 100644 index 000000000..5f9d0f56d --- /dev/null +++ b/src/modules/uORB/topics/actuator_direct.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 + * 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 actuator_direct.h + * + * Actuator direct values. + * + * Values published to this topic are the direct actuator values which + * should be passed to actuators, bypassing mixing + */ + +#ifndef TOPIC_ACTUATOR_DIRECT_H +#define TOPIC_ACTUATOR_DIRECT_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATORS_DIRECT 16 + +/** + * @addtogroup topics + * @{ + */ + +struct actuator_direct_s { + uint64_t timestamp; /**< timestamp in us since system boot */ + float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ + unsigned nvalues; /**< number of valid values */ +}; + +/** + * @} + */ + +/* actuator direct ORB */ +ORB_DECLARE(actuator_direct); + +#endif // TOPIC_ACTUATOR_DIRECT_H -- cgit v1.2.3 From b830137ec8492dc583b61689b7d56a1b359a5c5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:37:02 +1100 Subject: uavcan: added support for actuator_direct ORB topic this watches the actuator_direct topic and uses it to allow for direct output of actuator values, bypassing the mixer --- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 5 +++++ 2 files changed, 28 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ccc087920..cf40f3ea8 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -280,6 +280,7 @@ int UavcanNode::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -310,6 +311,18 @@ int UavcanNode::run() _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; + /* + * setup poll to look for actuator direct input if we are + * subscribed to the topic + */ + if (_actuator_direct_sub != -1) { + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; + _poll_fds[_poll_fds_num].events = POLLIN; + _actuator_direct_poll_fd_num = _poll_fds_num; + _poll_fds_num += 1; + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { @@ -342,6 +355,16 @@ int UavcanNode::run() } } + /* + see if we have any direct actuator updates + */ + if (_actuator_direct_sub != -1 && + (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { + // Output to the bus + _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + } + // can we mix? if (_test_in_progress) { float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b791663d4..d7eb16764 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -128,6 +129,10 @@ private: pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num; + actuator_direct_s _actuator_direct; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From ecc7a3cbb42f6490b43f21b3849738184934a2a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:52:50 +1100 Subject: motor_test: prevent use of uninitialised test_motor orb handle stack variables are not initialised to zero --- src/systemcmds/motor_test/motor_test.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 1b7ff75f7..77dc2f0d5 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,9 +59,10 @@ __EXPORT int motor_test_main(int argc, char *argv[]); static void motor_test(unsigned channel, float value); static void usage(const char *reason); +static orb_advert_t _test_motor_pub; + void motor_test(unsigned channel, float value) { - orb_advert_t _test_motor_pub; struct test_motor_s _test_motor; _test_motor.motor_number = channel; -- cgit v1.2.3 From ead0458e97d6e19cc0b188124063e90962820e3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 16:58:16 +1100 Subject: uavcan: don't force motors to keep spinning at zero throttle Forcing motors to keep spinning when armed should be a policy decision up at the vehicle type level, not hard coded down in the ESC driver. It isn't appropriate for fixed wing or ground vehicles for example. We could add an ioctl to enable "spin when armed" if just setting a small value in the vehicle code is inconvenient --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1d23099f3..7efd50511 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -101,10 +101,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). + if (scaled < 0.0F) { + scaled = 0.0F; + } if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); -- cgit v1.2.3 From 8e44ec2e3bd6852b74e4463c373555dd1bab47d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:00:02 +1100 Subject: uavcan: prevent crash in ESC driver passing in more than 8 actuators would crash the ESC driver. We need to check again the array size of the _esc_status.esc, which is CONNECTED_ESC_MAX --- src/modules/uavcan/actuators/esc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 7efd50511..9f682c7e1 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -76,7 +76,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } -- cgit v1.2.3 From 724ec0ec8b0d34c631b2784236a990e535700254 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:02:24 +1100 Subject: uavcan: handle all ESC output in one place moving all the ESC output handling to one place allows the limits on actuator values to apply to all types of inputs, and will make it easier to expand "uavcan status" to show actuator values --- src/modules/uavcan/uavcan_main.cpp | 80 +++++++++++++++++++++----------------- src/modules/uavcan/uavcan_main.hpp | 2 + 2 files changed, 47 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index cf40f3ea8..e4f58b310 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -282,8 +282,7 @@ int UavcanNode::run() _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) @@ -339,6 +338,8 @@ int UavcanNode::run() node_spin_once(); // Non-blocking + bool new_output = false; + // this would be bad... if (poll_ret < 0) { log("poll error %d", errno); @@ -360,18 +361,25 @@ int UavcanNode::run() */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && - orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { - // Output to the bus - _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + !_test_in_progress) { + if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], + _actuator_direct.nvalues*sizeof(float)); + _outputs.noutputs = _actuator_direct.nvalues; + new_output = true; } // can we mix? if (_test_in_progress) { - float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; - test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - - // Output to the bus - _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + _outputs.noutputs = _test_motor.motor_number+1; + } + new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -379,39 +387,41 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - outputs.timestamp = hrt_absolute_time(); - - // iterate actuators - for (unsigned i = 0; i < outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); - // limit outputs to valid range + new_output = true; + } + } - // never go below min - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - } + if (new_output) { + // iterate actuators, checking for valid values + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(_outputs.output[i])) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = -1.0f; + } - // never go below max - if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - } + // never go below min + if (_outputs.output[i] < -1.0f) { + _outputs.output[i] = -1.0f; } - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); + // never go above max + if (_outputs.output[i] > 1.0f) { + _outputs.output[i] = 1.0f; + } } + // Output to the bus + _outputs.timestamp = hrt_absolute_time(); + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); } + // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index d7eb16764..5a036fcd7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -133,6 +133,8 @@ private: uint8_t _actuator_direct_poll_fd_num; actuator_direct_s _actuator_direct; + actuator_outputs_s _outputs; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From a7a68c88a25c2c4cb401fb4f8894de6f4c280ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:03:29 +1100 Subject: uavcan: show ESC output values in uavcan status, and add arm/disarm this makes "uavcan status" show the current output values, which is useful for debugging. It also adds "uavcan arm" and "uavcan disarm" commands, which are very useful for re-arming after a motor test. --- src/modules/uavcan/uavcan_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e4f58b310..8fa765633 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -604,6 +604,14 @@ UavcanNode::print_info() (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + if (_outputs.noutputs != 0) { + printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i]*1000)); + } + printf("\n"); + } + // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { @@ -622,7 +630,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop}"); + "\tuavcan {start|status|stop|arm|disarm}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -669,6 +677,16 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "disarm")) { + inst->arm_actuators(false); + ::exit(0); + } + if (!std::strcmp(argv[1], "stop")) { delete inst; ::exit(0); -- cgit v1.2.3 From 7ae4f6d97e398a64aeb99c52880651a2282be5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Nov 2014 08:17:50 +1100 Subject: uavcan: added add_poll_fd() helper function this makes the code clearer and avoids repeated code --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++++++--------------- src/modules/uavcan/uavcan_main.hpp | 7 ++++++- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8fa765633..8147a8b89 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -269,6 +269,24 @@ void UavcanNode::node_spin_once() } } +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -304,22 +322,14 @@ int UavcanNode::run() * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ - _poll_fds_num = 0; - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; + add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; - _poll_fds[_poll_fds_num].events = POLLIN; - _actuator_direct_poll_fd_num = _poll_fds_num; - _poll_fds_num += 1; + _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { @@ -490,7 +500,6 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -503,10 +512,7 @@ UavcanNode::subscribe() } if (_control_subs[i] > 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_ids[i] = _poll_fds_num; - _poll_fds_num++; + _poll_ids[i] = add_poll_fd(_control_subs[i]); } } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 5a036fcd7..98f2e5ad4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -58,6 +58,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + /** * A UAVCAN node. */ @@ -98,6 +101,8 @@ private: int init(uavcan::NodeID node_id); void node_spin_once(); int run(); + int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -126,7 +131,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic -- cgit v1.2.3 From 809500cfcd220e65d7eb301e33533c5fe29f03d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:37:56 +1100 Subject: px4io: fixed RC_CONFIG channel limit check number of channels is the right test, not number of actuators --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f02ba62c..e61454b00 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2592,7 +2592,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= _max_actuators) { + if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ return E2BIG; } -- cgit v1.2.3 From 29f000dc31f6f18af0130b46d5e143e983549d19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:39:17 +1100 Subject: px4io: fixed error returns to be negative follow standard conventions --- src/drivers/px4io/px4io.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e61454b00..b31d7bbfa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2198,7 +2198,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); @@ -2217,7 +2217,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); @@ -2236,7 +2236,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); @@ -2255,7 +2255,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); @@ -2594,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ - return E2BIG; + return -E2BIG; } /* copy values to registers in IO */ -- cgit v1.2.3 From c0b47d6a74197a0dc57c56efbd63803424a9835a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Nov 2014 15:48:33 +1100 Subject: px4io: only check SAFETY_OFF for allowing RC config changes and reboot If we check OUTPUTS_ARMED then we can't update trim values and scaling in flight, as there is no way to clear OUTPUTS_ARMED. If safety is on then it should be perfectly safe to update the mixer and RC config or reboot for fw update --- src/modules/px4iofirmware/registers.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a1a02965f..f0c2cfd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -412,7 +412,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num * text handling function. */ return mixer_handle_text(values, num_values * sizeof(*values)); - break; default: /* avoid offset wrap */ @@ -584,10 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - // do not reboot if FMU is armed and IO's safety is off - // this state defines an active system. - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -633,12 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed - * = FMU is armed and IO's safety is off - * this state defines an active system. + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } -- cgit v1.2.3 From c906c2123822ef127026eeaf272b3aceed9f8995 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 09:22:24 +1100 Subject: px4io: prevent use of uninitialised memory in io_set_arming_state() the vehicle may not have setup a control_mode. We need to check the return of orb_copy() to ensure we are getting initialised values --- src/drivers/px4io/px4io.cpp | 74 +++++++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b31d7bbfa..58390ba4c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1160,52 +1160,54 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; ///< system armed state vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); + int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; - - } else { - clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; - } - - if (armed.lockdown && !_lockdown_override) { - set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } else { - clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } + if (have_armed == OK) { + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } - /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !_cb_flighttermination) { - set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } else { - clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !_cb_flighttermination) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } - if (armed.ready_to_arm) { - set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } - } else { - clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } } - if (control_mode.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - - } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + if (have_control_mode == OK) { + if (control_mode.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); -- cgit v1.2.3 From 8e932cec10aa7e64e4a5cd17a571778f3ff60e9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 20:00:40 +1100 Subject: systemcmds: added reflect command for USB testing --- src/systemcmds/reflect/module.mk | 41 +++++++++++++++ src/systemcmds/reflect/reflect.c | 111 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 src/systemcmds/reflect/module.mk create mode 100644 src/systemcmds/reflect/reflect.c diff --git a/src/systemcmds/reflect/module.mk b/src/systemcmds/reflect/module.mk new file mode 100644 index 000000000..973eb775d --- /dev/null +++ b/src/systemcmds/reflect/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 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 +# 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. +# +############################################################################ + +# +# Dump file utility +# + +MODULE_COMMAND = reflect +SRCS = reflect.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c new file mode 100644 index 000000000..6bb53c71a --- /dev/null +++ b/src/systemcmds/reflect/reflect.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2014 Andrew Tridgell. All rights reserved. + * + * 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 reflect.c + * + * simple data reflector for load testing terminals (especially USB) + * + * @author Andrew Tridgell + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int reflect_main(int argc, char *argv[]); + +// memory corruption checking +#define MAX_BLOCKS 1000 +static uint32_t nblocks; +struct block { + uint32_t v[256]; +}; +static struct block *blocks[MAX_BLOCKS]; + +#define VALUE(i) ((i*7) ^ 0xDEADBEEF) + +static void allocate_blocks(void) +{ + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + if (blocks[nblocks] == NULL) { + break; + } + for (uint32_t i=0; iv)/sizeof(uint32_t); i++) { + blocks[nblocks]->v[i] = VALUE(i); + } + nblocks++; + } + printf("Allocated %u blocks\n", nblocks); +} + +static void check_blocks(void) +{ + for (uint32_t n=0; nv)/sizeof(uint32_t); i++) { + assert(blocks[n]->v[i] == VALUE(i)); + } + } +} + +int +reflect_main(int argc, char *argv[]) +{ + uint32_t total = 0; + printf("Starting reflector\n"); + + allocate_blocks(); + + while (true) { + char buf[128]; + ssize_t n = read(0, buf, sizeof(buf)); + if (n < 0) { + break; + } + if (n > 0) { + write(1, buf, n); + } + total += n; + if (total > 1024000) { + check_blocks(); + total = 0; + } + } + return OK; +} -- cgit v1.2.3 From 4724c050478900a0b0b760877f1613e43c0aa97c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Nov 2014 11:24:28 +1100 Subject: airspeed: use _retries=2 for I2C retries once initialised airspeed sensors often need to be on longer cables due to having to be outside the prop wash. --- src/drivers/airspeed/airspeed.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 3a1e1b7b5..6db6713c4 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -159,13 +159,15 @@ out: int Airspeed::probe() { - /* on initial power up the device needs more than one retry - for detection. Once it is running then retries aren't - needed + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced */ _retries = 4; int ret = measure(); - _retries = 0; + + // drop back to 2 retries once initialised + _retries = 2; return ret; } -- cgit v1.2.3