diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.logging | 2 | ||||
-rw-r--r-- | makefiles/config_aerocore_default.mk | 2 | ||||
-rw-r--r-- | nuttx-configs/px4fmu-v1/nsh/defconfig | 2 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/frsky_data.c | 2 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/frsky_telemetry.c | 2 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/module.mk | 2 | ||||
-rw-r--r-- | src/drivers/gps/gps.cpp | 12 | ||||
-rw-r--r-- | src/drivers/gps/module.mk | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 21 | ||||
-rw-r--r-- | src/examples/flow_position_estimator/flow_position_estimator_main.c | 5 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/module.mk | 2 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 13 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 24 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 16 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 14 | ||||
-rw-r--r-- | src/systemcmds/nshterm/module.mk | 2 |
19 files changed, 94 insertions, 35 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 009e7431a..517e073af 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 5 -t + sdlog2 start -r 50 -a -b 4 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index e038500a6..d6902c4ee 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -53,7 +53,7 @@ MODULES += modules/mavlink # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav # diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index c599b118f..bc0c12067 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -418,7 +418,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_USERMAIN_STACKSIZE=3500 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 57a03bc84..18de3f4da 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,6 +53,8 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_status.h> +#include <drivers/drv_hrt.h> + /* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 7b08ca69e..6e0839043 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[]) frsky_task = task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 2000, frsky_telemetry_thread_main, (const char **)argv); diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 1632c74f7..9a49589ee 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry SRCS = frsky_data.c \ frsky_telemetry.c + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2d8d37298..6195cd6ea 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -56,6 +56,7 @@ #include <arch/board/board.h> #include <drivers/drv_hrt.h> #include <drivers/device/i2c.h> +#include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> #include <systemlib/scheduling_priorities.h> #include <systemlib/err.h> @@ -78,12 +79,6 @@ #endif static const int ERROR = -1; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - - - class GPS : public device::CDev { public: @@ -211,7 +206,8 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 82c67d40a..eb382c4b2 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -41,3 +41,5 @@ SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ ubx.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3c3b59862..4099e5522 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -794,7 +794,12 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_spawn_cmd("px4io", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 2000, + (main_t)&PX4IO::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -989,13 +994,17 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) { + if (failsafe_param != PARAM_INVALID) { param_get(failsafe_param, &failsafe_param_val); - uint16_t failsafe_thr = failsafe_param_val; - pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); - if (pret != OK) { - log("failsafe upload failed"); + + if (failsafe_param_val > 0) { + + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed, FS: %d us", (int)failsafe_thr); + } } } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 495c415f2..c775428ef 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -65,6 +65,7 @@ #include <uORB/topics/optical_flow.h> #include <uORB/topics/filtered_bottom_flow.h> #include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> #include <poll.h> #include "flow_position_estimator_params.h" @@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_RR, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4096, + 4000, flow_position_estimator_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 199e85305..6c97bfca7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2204,7 +2204,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2000, + 1950, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b03a68c07..72b9ee83a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -949,7 +949,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 3000); + pthread_attr_setstacksize(&receiveloop_attr, 2900); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 939d76849..0658d3f09 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = position_estimator_inav SRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ inertial_filter.c + +MODULE_STACKSIZE = 1200 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 368424853..d8d0ff37d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -135,7 +135,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 91975f2a0..7471faec7 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -190,6 +190,8 @@ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#else +#define PX4IO_P_SETUP_RELAYS_PAD 5 #endif #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ @@ -209,15 +211,16 @@ enum { /* DSM bind states */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* 12 occupied by CRC */ + /* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ -#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into - 'armed' (PWM enabled) state */ -#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ #define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fd7c6081f..db1836f4a 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -160,6 +160,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_ALTRATE] = 200, #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#else + /* this is unused, but we will pad it for readability (the compiler pads it automatically) */ + [PX4IO_P_SETUP_RELAYS_PAD] = 0, #endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, @@ -523,18 +526,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 50) + if (value < 50) { value = 50; - if (value > 400) + } + if (value > 400) { value = 400; + } pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_ALTRATE: - if (value < 50) + if (value < 50) { value = 50; - if (value > 400) + } + if (value > 400) { value = 400; + } pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; @@ -566,8 +573,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) } // check the magic value - if (value != PX4IO_REBOOT_BL_MAGIC) + if (value != PX4IO_REBOOT_BL_MAGIC) { break; + } // we schedule a reboot rather than rebooting // immediately to allow the IO board to ACK @@ -585,6 +593,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) } break; + case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: + if (value > 650 && value < 2350) { + r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; + } + break; + default: return -1; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b74d4183b..39f433eb5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -684,7 +684,7 @@ int sdlog2_thread_main(int argc, char *argv[]) case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r <= 0) { + if (r == 0) { r = 1; } @@ -834,6 +834,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_GSN0_s log_GSN0; + struct log_GSN1_s log_GSN1; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -982,6 +984,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; LOGBUFFER_WRITE_AND_COUNT(GPS); + + /* log the SNR of each satellite for a detailed view of signal quality */ + log_msg.msg_type = LOG_GSN0_MSG; + /* pick the smaller number so we do not overflow any of the arrays */ + unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]); + unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < sat_max_snr; i++) { + log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + } + LOGBUFFER_WRITE_AND_COUNT(GSN0); } /* --- SENSOR COMBINED --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 595a787d6..0c6188657 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -317,6 +317,18 @@ struct log_VICN_s { float yaw; }; +/* --- GSN0 - GPS SNR #0 --- */ +#define LOG_GSN0_MSG 26 +struct log_GSN0_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + +/* --- GSN1 - GPS SNR #1 --- */ +#define LOG_GSN1_MSG 27 +struct log_GSN1_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -368,6 +380,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index fe79ec3b7..b22b446da 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1500 +MODULE_STACKSIZE = 1200 |