From cb3a4f12670ce8d6cf608eb0ec97298a3bcb0919 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044..2c0078503 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ -- cgit v1.2.3 From 409fa565f48ffe164b7332c9186a876b2771922a Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 158 +++++++++++++++++------------------ 1 file changed, 77 insertions(+), 81 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d..609dd3312 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { - - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | -- cgit v1.2.3 From ed7b97c0203fd1a16768222cad6c6f49b0534bd2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..58995fcec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -592,11 +592,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From f17c0b133559dc440a7789caa45767b95de84e7b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:15:47 +0200 Subject: mavlink: implemented multicasting between mavlink instances (two options: forwarding: forward received messages from self to other mavlink instances, passing: send out messages received from other mavlink intances over serial --- src/modules/mavlink/mavlink_main.cpp | 197 ++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 34 +++++- src/modules/mavlink/mavlink_messages.cpp | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 5 + 4 files changed, 236 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fe..c5055939e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -207,10 +207,15 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +266,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + /* don't broadcast to itself */ + if (inst != self) { + inst->pass_message(msg); + } + inst = inst->next; + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -1616,6 +1634,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } +int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char*)malloc(_message_buffer.size); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + int Mavlink::task_main(int argc, char *argv[]) { @@ -1632,7 +1769,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; @@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2067,7 +2258,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..4f9a53a5b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,12 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -234,6 +238,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +258,16 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + /** * Send one parameter. * @@ -315,6 +331,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..2b5d65080 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,11 +1265,13 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670..1581f30d3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -913,6 +913,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } -- cgit v1.2.3 From c6d98a32f83383e6204fd6cefbfcc1fd7e1cf159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:27:43 +0200 Subject: Proper failsafe handling onboard, including throttle failsafe condition if enabled --- src/modules/px4iofirmware/controls.c | 36 ++++++++++++++++++++++++------------ src/modules/px4iofirmware/protocol.h | 9 +++++---- 2 files changed, 29 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd3312..b860fc587 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); @@ -261,8 +259,20 @@ controls_tick() { if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; + } + + if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); @@ -312,6 +322,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -322,27 +337,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - - /* Set channel count to zero */ - r_raw_rc_count = 0; - } - /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c529..b69f68107 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -164,10 +164,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ -#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ -- cgit v1.2.3 From 64ffafb48ee45452070dfb37be8d0de6098915a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:28:01 +0200 Subject: Only publish RC inputs if we have seen some valid inputs at some point --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c0078503..8eee1cbca 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ -- cgit v1.2.3 From fb44ad8e22f2a0862f1d7bda66643a6336247024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:29:17 +0200 Subject: Simplify the failsafe handling, reduce 3 params to one --- src/modules/sensors/sensor_params.c | 28 +++------------------------- src/modules/sensors/sensors.cpp | 26 +++++++------------------- 2 files changed, 10 insertions(+), 44 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a..e4564aa25 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - -/** - * Failsafe channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_CH, 0); - -/** - * Failsafe channel mode. - * - * 0 = too low means signal loss, - * 1 = too high means signal loss - * - * @min 0 - * @max 1 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); - /** * Failsafe channel PWM threshold. * - * @min 0 - * @max 1 + * @min 800 + * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); +PARAM_DEFINE_FLOAT(RC_FS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f..2442acd6b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,8 +263,6 @@ private: float rc_scale_yaw; float rc_scale_flaps; - int rc_fs_ch; - int rc_fs_mode; float rc_fs_thr; float battery_voltage_scaling; @@ -313,8 +311,6 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; - param_t rc_fs_ch; - param_t rc_fs_mode; param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -531,8 +527,6 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); /* gyro offsets */ @@ -689,8 +683,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); - param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); - param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ @@ -1312,19 +1304,15 @@ Sensors::rc_poll() manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) + if (rc_input.channel_count < 4) { return; + } - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; - } + /* check for failsafe */ + if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + /* do not publish manual control setpoints when there are none */ + return; } unsigned channel_limit = rc_input.channel_count; -- cgit v1.2.3 From 797698a7a114032c7eea62c5f48f2b229ca973b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:34:35 +0200 Subject: Trigger failsafe action also on failsafe flag --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2442acd6b..4c34d853f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,7 +1309,7 @@ Sensors::rc_poll() } /* check for failsafe */ - if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { /* do not publish manual control setpoints when there are none */ return; -- cgit v1.2.3 From 9123ebce8cbc618899ece31d31c97e022038beb2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:45:02 +0200 Subject: px4io: Allow RC failsafe detection as valid feature --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa..42b863b53 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -175,7 +175,8 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ + PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif -- cgit v1.2.3 From 9a0b2b7610d39f88b627046e0d90f66aada1e88f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:02:22 +0200 Subject: Make throttle failsafe depend on the failsafe threshold parameter. Make the parameter optional (no harm if not found). --- src/drivers/px4io/px4io.cpp | 17 ++++++++++++++++- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/protocol.h | 21 +++++++++++---------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++-- 5 files changed, 33 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8eee1cbca..f6125d273 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -944,8 +944,23 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); } + + /* send RC throttle failsafe value to IO */ + float failsafe_param_val; + param_t failsafe_param = param_find("RC_FS_THR"); + + if (failsafe_param > 0) + + 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"); + } + } + } } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b860fc587..356fe44cd 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -264,10 +264,10 @@ controls_tick() { scaled = -scaled; } - if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + if (mapped == 3 && r_setup_rc_thr_failsafe) { /* throttle failsafe detection */ - if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || - ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b69f68107..a978d483a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -168,7 +168,6 @@ #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ #define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ #define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -202,13 +201,15 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + /* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ -#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ -#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ -#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* 12 occupied by CRC */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -218,10 +219,10 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5..88ad79398 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42b863b53..6752e7b4b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,14 +169,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ - PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif -- cgit v1.2.3 From 5e0d687b566022c12270f68facbf7ca35f62306c Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044..2c0078503 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ -- cgit v1.2.3 From 745ef4f4856af60aca5625860e5e3f25ea189dc9 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 158 +++++++++++++++++------------------ 1 file changed, 77 insertions(+), 81 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d..609dd3312 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { - - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | -- cgit v1.2.3 From 09d106432749ec02866241863eb912f05b903e64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:06:07 +0200 Subject: px4io: Remove unused variable --- src/modules/px4iofirmware/controls.c | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd3312..f56f630bc 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); -- cgit v1.2.3 From 3e4841b6fe2d8d6d06b167be49cbe76ab7e04a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:10:41 +0200 Subject: px4io: Guard against the RC failsafe value of channel 5 causing a manual override action if set to manual in failsafe --- src/modules/px4iofirmware/controls.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f56f630bc..aae7bb951 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -310,6 +310,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -320,27 +325,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - } - - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - /* Set channel count to zero */ r_raw_rc_count = 0; + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -363,10 +365,10 @@ controls_tick() { mixer_tick(); } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } -- cgit v1.2.3 From 73d04f7a37ba52ce1891e740db554557bd71940a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:15:22 +0200 Subject: px4io driver: Only publish RC signal if it was at least once valid. --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c0078503..8eee1cbca 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ -- cgit v1.2.3 From 3b5e6f98338fded2cbe7be1c301dc2698f7239aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:28:07 +0200 Subject: sensors and px4io driver: Guard against failsafe trigger for inverted remotes --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6125d273..a30bfb2de 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -948,8 +948,8 @@ PX4IO::task_main() } /* send RC throttle failsafe value to IO */ - float failsafe_param_val; - param_t failsafe_param = param_find("RC_FS_THR"); + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); if (failsafe_param > 0) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index e4564aa25..14f7ac812 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -675,4 +675,4 @@ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 0); +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4c34d853f..44a91ca67 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,7 +263,7 @@ private: float rc_scale_yaw; float rc_scale_flaps; - float rc_fs_thr; + int32_t rc_fs_thr; float battery_voltage_scaling; float battery_current_scaling; @@ -527,7 +527,7 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { /* do not publish manual control setpoints when there are none */ return; } -- cgit v1.2.3 From ea5279389f2b13110735083f511afb630ef5e3d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:16:51 +1100 Subject: uORB: added an ORB topic for system_power holds power supply state and 5V rail voltage on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/system_power.h | 71 ++++++++++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/system_power.h (limited to 'src') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a4..c8a589bb7 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 000000000..7763b6004 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 system_power.h + * + * Definition of the system_power voltage and power status uORB topic. + */ + +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * voltage and power supply status + */ +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif -- cgit v1.2.3 From 6ea22c8c079db8633d663cbf8ca39b81a434a040 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:17:25 +1100 Subject: adc: publish system_power ORB topic from ADC --- src/drivers/stm32/adc/adc.cpp | 47 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 0b8a275e6..3a60d2cae 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include @@ -64,6 +65,8 @@ #include #include +#include + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -119,6 +122,8 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ + orb_advert_t _to_system_power; + /** work trampoline */ static void _tick_trampoline(void *arg); @@ -134,13 +139,16 @@ private: */ uint16_t _sample(unsigned channel); + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); }; ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), _channel_count(0), - _samples(nullptr) + _samples(nullptr), + _to_system_power(0) { _debug_enabled = true; @@ -290,6 +298,43 @@ ADC::_tick() /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power > 0) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } uint16_t -- cgit v1.2.3 From da105650526f724247aa8db7ee87a460efcd3cd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 12:42:32 +0200 Subject: sdlog2: Add system power to logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 39 insertions(+) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac54..7a984821f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -796,6 +796,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct system_power_s system_power; + struct servorail_status_s servorail_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -828,6 +830,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; + struct log_PWR_s log_PWR; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -859,6 +862,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int system_power_sub; + int servorail_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -884,6 +889,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); + subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); thread_running = true; @@ -1226,6 +1233,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- SYSTEM POWER RAILS --- */ + if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + log_msg.msg_type = LOG_PWR_MSG; + log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; + log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; + log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; + log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; + log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; + + /* copy servo rail status topic here too */ + orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); + log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + + LOGBUFFER_WRITE_AND_COUNT(PWR); + } + /* --- TELEMETRY --- */ if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { log_msg.msg_type = LOG_TELE_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 22a695872..93a150dbe 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -316,6 +316,19 @@ struct log_ESTM_s { // uint8_t kalman_gain_nan; // }; +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 133 +struct log_PWR_s { + float 5v_peripherals; + float 5v_servo_rail; + float 5v_servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -349,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; -- cgit v1.2.3 From 06d5c6ad285a46cfb59ce07bc3cd49507305a997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 13:03:52 +0200 Subject: sdlog2: Compile fixes for system power logging --- src/modules/sdlog2/sdlog2.c | 8 +++++--- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 2 files changed, 9 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7a984821f..9f2fca446 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,6 +84,8 @@ #include #include #include +#include +#include #include #include @@ -1236,7 +1238,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SYSTEM POWER RAILS --- */ if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; - log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; @@ -1245,8 +1247,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); - log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93a150dbe..d629259c0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -319,9 +319,9 @@ struct log_ESTM_s { /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 133 struct log_PWR_s { - float 5v_peripherals; - float 5v_servo_rail; - float 5v_servo_rssi; + float peripherals_5v; + float servo_rail_5v; + float servo_rssi_5v; uint8_t usb_ok; uint8_t brick_ok; uint8_t servo_ok; @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; -- cgit v1.2.3 From 00c82f0836388ffa1e1c4a5827eefe28c0521df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:18:20 +1100 Subject: Merged airspeed changes --- src/drivers/meas_airspeed/meas_airspeed.cpp | 62 +++++++++++++++++++++++++++-- 1 file changed, 59 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2c3efdc35..2324475ce 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include @@ -121,6 +122,14 @@ protected: virtual int collect(); math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -129,10 +138,11 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -207,6 +217,9 @@ MEASAirspeed::collect() diff_press_pa = 0.0f; } + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa); + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -287,6 +300,49 @@ MEASAirspeed::cycle() USEC2TICK(CONVERSION_INTERVAL)); } +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 70.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + /** * Local functions in support of the shell command. */ -- cgit v1.2.3 From 9719af623d92ceab0c5eebe21906b4d5cf515682 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 13:48:37 +1100 Subject: Merged voltage compensation --- src/drivers/meas_airspeed/meas_airspeed.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ce..0b7986383 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - float temperature = ((200 * dT_raw) / 2047) - 50; + float temperature = ((200.0f * dT_raw) / 2047) - 50; /* calculate differential pressure. As its centered around 8000 * and can go positive or negative, enforce absolute value @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } -- cgit v1.2.3 From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 +++++++++++++++++++------ src/modules/uORB/topics/differential_pressure.h | 9 +++-- 2 files changed, 43 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383..58b128948 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6..01e14cda9 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From 3da219c3db638e0a57d18e892575df13d8c11f47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:14:03 +0200 Subject: Update airspeed calibration routine to account for new signedness options --- src/modules/commander/airspeed_calibration.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a7..c8c7a42e7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd) if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) -- cgit v1.2.3 From be38372be17925e14fa864ad26e8f487a402d46f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:28:44 +0200 Subject: sf0x driver: Stop emitting error messages once there is no hope this sensor will recover - continue to output error messages if the errors are just intermittent --- src/drivers/sf0x/sf0x.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 07163035b..a0cf98340 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -124,6 +124,8 @@ private: orb_advert_t _range_finder_topic; + unsigned _consecutive_fail_count; + perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _last_read(0), _range_finder_topic(-1), + _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) @@ -720,12 +723,17 @@ SF0X::cycle() if (OK != collect_ret) { /* we know the sensor needs about four seconds to initialize */ - if (hrt_absolute_time() > 5 * 1000 * 1000LL) { - log("collection error"); + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + log("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; + /* restart the measurement state machine */ start(); return; + } else { + /* apparently success */ + _consecutive_fail_count = 0; } /* next phase is measurement */ -- cgit v1.2.3 From 671d35f67ccae5c35ef4fcb573f5fa52395601c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:05:58 +0200 Subject: Fix logic for S.Bus failsafe detection --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 44a91ca67..c91006262 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } -- cgit v1.2.3 From de7a6b057f5ced2cf118a919912327f878492cdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:08:28 +0200 Subject: Cleanups on system power logging format --- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/sdlog2/sdlog2_messages.h | 64 ++++++++++++++++-------------------- 2 files changed, 29 insertions(+), 37 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f2fca446..d2cf6d662 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1248,7 +1248,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d629259c0..354bb08e8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -277,6 +277,29 @@ struct log_TELE_s { uint8_t txbuf; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 23 +struct log_ESTM_s { + float s[10]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 24 +struct log_PWR_s { + float peripherals_5v; + float servo_rail_5v; + float servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -299,36 +322,6 @@ struct log_PARM_s { float value; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 132 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; -// struct log_ESTM_s { -// float s[32]; -// uint8_t n_states; -// uint8_t states_nan; -// uint8_t covariance_nan; -// uint8_t kalman_gain_nan; -// }; - -/* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 133 -struct log_PWR_s { - float peripherals_5v; - float servo_rail_5v; - float servo_rssi_5v; - uint8_t usb_ok; - uint8_t brick_ok; - uint8_t servo_ok; - uint8_t low_power_rail_overcurrent; - uint8_t high_power_rail_overcurrent; -}; - #pragma pack(pop) /* construct list of all message formats */ @@ -355,17 +348,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), /* system-level messages, ID >= 0x80 */ - // FMT: don't write format of format message, it's useless + /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), - LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PARM, "Nf", "Name,Value") }; -static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); +static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]); #endif /* SDLOG2_MESSAGES_H_ */ -- cgit v1.2.3 From 7b95d36405cb63b53fd1fea2c25e29aedca5a3a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:45:23 +0200 Subject: navigator hotfix: Increase acceptance range for yaw setpoints. --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..5a94b6671 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } -- cgit v1.2.3 From fc39af08a1d6673aa727a84b17afd6c4485dff19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:02:37 +0200 Subject: airspeed: Prevent the filter from overshooting into the negative airspeed range --- src/drivers/meas_airspeed/meas_airspeed.cpp | 6 ++++++ src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 7 ++++++- src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 20 ++++++++++++++++---- 3 files changed, 28 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 58b128948..1ad383ee0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,6 +258,12 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + + /* the dynamics of the filter can make it overshoot into the negative range */ + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce..6f640c9f9 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4..74cd5d78c 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; -- cgit v1.2.3 From 024de1fec431fbd065aeb31035245e7851450a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:04:36 +0200 Subject: Remove unwanted colon --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad79398..4db948484 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) -- cgit v1.2.3 From 262485a5e87ccdc8ff645ba45992bdbe13363fab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:09:48 +0200 Subject: px4io: Typo fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2de..e318e206a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,7 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; -- cgit v1.2.3 From e6d48c4f3276af252315118286556b3a000274b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:12:21 +0200 Subject: Fix failsafe assignment in sensors app --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c91006262..b171b995d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } -- cgit v1.2.3 From 6319ec2036c52f39a6fded6480836fe79e3ba35f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:34 +0200 Subject: Add celsius air temperature field to airspeed --- src/modules/uORB/topics/airspeed.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758f..d2ee754cd 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** -- cgit v1.2.3 From c17201afbff06f2be278e202d23eb136f38d1ae1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:54 +0200 Subject: Log air temperature --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d2cf6d662..e62b0fafc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; LOGBUFFER_WRITE_AND_COUNT(AIRS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 354bb08e8..a1a5856bc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -174,6 +174,7 @@ struct log_OUT0_s { struct log_AIRS_s { float indicated_airspeed; float true_airspeed; + float air_temperature_celsius; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ @@ -338,7 +339,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), -- cgit v1.2.3 From a1a4013d02ca7ec3f62c6c3f2e4b95181f365c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:24:11 +0200 Subject: Populate air temperature field --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b171b995d..4083b8b4f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1025,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celcius); + raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { -- cgit v1.2.3 From 4a0c6600887e900932f6888f1b8948816a1f00b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:06:21 +0200 Subject: Do not make minimum airspeed assumptions, as we can trust our digital sensor a bit. A blocked pitot also most likely will result in more than just 6.5 m/s airspeed and so the check is based on a bunch of weak assumptions --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f4..522e8c3e6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,14 +660,13 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } float airspeed_scaling = _parameters.airspeed_trim / airspeed; -- cgit v1.2.3 From ab60b13b6dbcf636b1889d2150d96aff8b26cfc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:26:17 +0200 Subject: fw_att_controller: Forcing actuator scaling to at least minimum speed --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 522e8c3e6..2f84dc963 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -669,8 +669,15 @@ FixedwingAttitudeControl::task_main() airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; -- cgit v1.2.3 From 7c4f1c90dc441e57d9af55beb33bb6d91ece0c90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 14:57:45 +0400 Subject: mavlink_receiver: fixed bug in manual control publication, minor refactoring --- src/modules/mavlink/mavlink_receiver.cpp | 60 +++++++++++++++----------------- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 29 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670..3ec40ee0a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7..72ce4560f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; -- cgit v1.2.3 From e4cfdb4f9f30e0eef36f9e18e8d656d57994141e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Apr 2014 19:34:32 +0200 Subject: mavlink: Add manual SP subscription --- src/modules/mavlink/mavlink_receiver.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 72ce4560f..8ccb2a035 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; -- cgit v1.2.3 From 536ff50fe1ddc438283f5f3c66ec9dc3d79d7d83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Apr 2014 14:38:16 +0200 Subject: mavlink: in normal mode transmit position setpoint and roll-pitch-yaw-thrust setpoint --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fe..16c27baa9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1783,6 +1783,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); break; case MAVLINK_MODE_CAMERA: -- cgit v1.2.3 From de3efc0975f1b84cf556bae11bbebe95609dbcdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:17:56 +0400 Subject: mavlink: publish SYS_STATUS at constant rate, don't look at update() result --- src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..c89031fcc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; -- cgit v1.2.3 From a4a12dab337f132bfe51db2ad482273ecfdf0ce6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 58995fcec..06f7ae6cc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -588,15 +588,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From bb3792bcdd8edeb24cad7ef0170c76a552f943d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:48 +0200 Subject: mavlink: use LL_FOREACH --- src/modules/mavlink/mavlink_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c5055939e..58393e013 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -401,14 +401,12 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - /* don't broadcast to itself */ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { inst->pass_message(msg); } - inst = inst->next; } } -- cgit v1.2.3 From 38c3e68976c8dc167b4d1e5d24792401fc7cc7d3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:44:01 +0200 Subject: Send camera command to all, use own sysid --- src/modules/mavlink/mavlink_messages.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2b5d65080..1e6bd6f93 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,13 +1265,11 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From 814d3c385cc0132ec9bbdc22f87f35301bbc8f44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 11:03:23 +0200 Subject: gps driver: fake mode: lower eph and epv values in order to convince the commander that the gps signal is valid --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c72f692d7..a902bdf2f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; + _report.eph_m = 3.0f; + _report.epv_m = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; -- cgit v1.2.3 From 134633c7f0ef6ec668ba9c122a52b024c393e51a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 15:15:06 +0200 Subject: mavlink: accessor for _mavlink_fd --- src/modules/mavlink/mavlink_main.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..4e6d585ab 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -196,6 +196,8 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ + int get_mavlink_fd() { return _mavlink_fd; } + protected: Mavlink *next; -- cgit v1.2.3 From a6215b7bda47616018d29b2f7de630deb4984be9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: commander: handle_command: filter commands by sysid and componentid --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 69a45a02f..89b8e684d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ -- cgit v1.2.3 From f0d043c06849a2f2f5e07d6f3bdfafc0e4e6f041 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:08:35 +0200 Subject: mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic and send mavlink_msg_command_long --- src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497..4d7b9160d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1273,6 +1273,51 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() + { + return "COMMAND_LONG"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCommandLong(); + } + +private: + MavlinkOrbSubscription *vehicle_command_sub; + struct vehicle_command_s *vehicle_command; + +protected: + void subscribe(Mavlink *mavlink) + { + vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (vehicle_command_sub->update(t)) { + if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { + mavlink_msg_command_long_send(_channel, + vehicle_command->target_system, + vehicle_command->target_component, + vehicle_command->command, + vehicle_command->confirmation, + vehicle_command->param1, + vehicle_command->param2, + vehicle_command->param3, + vehicle_command->param4, + vehicle_command->param5, + vehicle_command->param6, + vehicle_command->param7); + } + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From bccdbde45eb70271bd946ca86173cd8add20cd3d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: commander: handle_command: do not filter command if componentid == 0 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89b8e684d..ce6de88ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } -- cgit v1.2.3 From 523606668f8c940357cd9c46b0995035eced7659 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 22:23:18 +0400 Subject: position_estimator_inav: make land detector more sensitive to LANDED -> IN AIR transitions --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src') 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 368fa6ee2..763b87563 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp2 > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 || thrust > params.land_thr) { landed = false; landed_time = 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f..dcad5c03b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { -- cgit v1.2.3 From 09093b17daf8f02f87ea689dc73bd35b6cac1542 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:28:52 +0400 Subject: mavlink: commands stream implemented --- src/modules/mavlink/mavlink_commands.cpp | 73 ++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_commands.h | 65 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.cpp | 6 +++ src/modules/mavlink/module.mk | 3 +- 4 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 src/modules/mavlink/mavlink_commands.cpp create mode 100644 src/modules/mavlink/mavlink_commands.h (limited to 'src') diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp new file mode 100644 index 000000000..1c1e097a4 --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 mavlink_commands.cpp + * Mavlink commands stream implementation. + * + * @author Anton Babushkin + */ + +#include "mavlink_commands.h" + +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +{ + _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); +} + +MavlinkCommandsStream::~MavlinkCommandsStream() +{ +} + +void +MavlinkCommandsStream::update(const hrt_abstime t) +{ + if (_cmd_sub->update(t)) { + /* only send commands for other systems/components */ + if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + mavlink_msg_command_long_send(_channel, + _cmd->target_system, + _cmd->target_component, + _cmd->command, + _cmd->confirmation, + _cmd->param1, + _cmd->param2, + _cmd->param3, + _cmd->param4, + _cmd->param5, + _cmd->param6, + _cmd->param7); + } + } +} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h new file mode 100644 index 000000000..6255d65df --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 mavlink_commands.h + * Mavlink commands stream definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_COMMANDS_H_ +#define MAVLINK_COMMANDS_H_ + +#include +#include + +class Mavlink; +class MavlinkCommansStream; + +#include "mavlink_main.h" + +class MavlinkCommandsStream +{ +private: + MavlinkOrbSubscription *_cmd_sub; + struct vehicle_command_s *_cmd; + mavlink_channel_t _channel; + +public: + MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); + ~MavlinkCommandsStream(); + void update(const hrt_abstime t); +}; + +#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3d897431a..1ed3f4001 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,6 +81,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_commands.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -1920,6 +1921,8 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + MavlinkCommandsStream commands_stream(this, _channel); + /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; @@ -1982,6 +1985,9 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status->hil_state == HIL_STATE_ON); } + /* update commands stream */ + commands_stream.update(t); + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f09efa2e6..515fbfadc 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -42,6 +42,7 @@ SRCS += mavlink_main.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From d55e64d1e54542762510387a22897f504c68a5a6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:29:40 +0400 Subject: mavlink: minor comments and formatting fixes --- src/modules/mavlink/mavlink_messages.cpp | 2 -- src/modules/mavlink/mavlink_stream.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497..47603b390 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1252,8 +1252,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..def40d9ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin -- cgit v1.2.3 From 27884e49bedfdbd0e259bb9b90e757b45bf7fd44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Apr 2014 21:40:05 +0200 Subject: Revert "Merge pull request #816 from PX4/mavlink_commandlongstream" This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing changes made to d55e64d1e54542762510387a22897f504c68a5a6. --- src/modules/mavlink/mavlink_messages.cpp | 46 -------------------------------- 1 file changed, 46 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bb8bba037..47603b390 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1271,51 +1271,6 @@ protected: } }; -class MavlinkStreamCommandLong : public MavlinkStream -{ -public: - const char *get_name() - { - return "COMMAND_LONG"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCommandLong(); - } - -private: - MavlinkOrbSubscription *vehicle_command_sub; - struct vehicle_command_s *vehicle_command; - -protected: - void subscribe(Mavlink *mavlink) - { - vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (vehicle_command_sub->update(t)) { - if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { - mavlink_msg_command_long_send(_channel, - vehicle_command->target_system, - vehicle_command->target_component, - vehicle_command->command, - vehicle_command->confirmation, - vehicle_command->param1, - vehicle_command->param2, - vehicle_command->param3, - vehicle_command->param4, - vehicle_command->param5, - vehicle_command->param6, - vehicle_command->param7); - } - } - } -}; - MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamCommandLong(), nullptr }; -- cgit v1.2.3 From 63dae04c22009512abe9a0450480698de2bba9db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Apr 2014 17:06:06 +0200 Subject: pauls estimator: Added NaN guard before publishing --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585e..8ea611802 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -959,7 +959,7 @@ FixedwingEstimator::task_main() } // Publish results - if (_initialized) { + if (_initialized && (check == OK)) { -- cgit v1.2.3 From 863cd2e838a5e30efd817520c3fceb3847dd18f6 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:11:27 +0200 Subject: Added VICON logging, finally. --- src/modules/sdlog2/sdlog2.c | 30 +++++++++++++++++++----------- src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++++ 2 files changed, 31 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e62b0fafc..83ee93408 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -226,11 +226,11 @@ sdlog2_usage(const char *reason) } errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; + struct log_VICON_s log_VICON; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - // TODO not implemented yet + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a1a5856bc..eee5ae590 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -301,6 +301,17 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; +/* --- VICON - VICON POSITION --- */ +#define LOG_VICON_MSG 25 +struct log_VICON_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), + LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 284218174cad1817b5dc542ecb1527f6ec58bb2d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:21:07 +0200 Subject: Tabs tabs tabs --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 83ee93408..8dfe17228 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ -- cgit v1.2.3 From 5b090a7095a25f31dd040935defa2363a60b3107 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:25:33 +0200 Subject: VICON -> VICN --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index eee5ae590..affd512cc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From a39382dc174d646c5ecda8c15c6e0db9c1c4e703 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:28:00 +0200 Subject: VICON -> VICN really this time. --- src/modules/sdlog2/sdlog2.c | 18 +++++++++--------- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8dfe17228..1bf6f1de3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -833,7 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; - struct log_VICON_s log_VICON; + struct log_VICN_s log_VICN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICN_MSG; + log_msg.body.log_VICN.x = buf.vicon_pos.x; + log_msg.body.log_VICN.y = buf.vicon_pos.y; + log_msg.body.log_VICN.z = buf.vicon_pos.z; + log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICN.roll = buf.vicon_pos.roll; + log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICN); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index affd512cc..d0585df39 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -301,9 +301,9 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; -/* --- VICON - VICON POSITION --- */ -#define LOG_VICON_MSG 25 -struct log_VICON_s { +/* --- VICN - VICON POSITION --- */ +#define LOG_VICN_MSG 25 +struct log_VICN_s { float x; float y; float z; -- cgit v1.2.3 From e99291d825cfc89c67cadd8487e568d3b21d218f Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:37 +0200 Subject: Added vicon stream. --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..37929edac 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -640,6 +640,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; -- cgit v1.2.3 From e6542653b9d013d5cb1b1c0f01ee9af7de4abe5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:15 +0200 Subject: Finished adding a '-w' option. --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 12 +++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ 3 files changed, 40 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f4001..b9e0663b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + // XXX do something here, but change to using FIONWRITE and OS buf size for detection + } } + + } static void usage(void); @@ -204,6 +209,8 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -1768,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1820,6 +1827,10 @@ Mavlink::task_main(int argc, char *argv[]) _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -2164,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance @@ -2264,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9941a5f99..2c1826139 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -202,6 +202,14 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -216,6 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -270,6 +280,8 @@ private: pthread_mutex_t _message_buffer_mutex; + + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c66350f5b..61ef2b043 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void -- cgit v1.2.3 From 4f91fdb98ca03f27ea130a53636b4b7990fba183 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:30:00 +0200 Subject: Indentation. --- src/modules/mavlink/mavlink_main.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2c1826139..66d82b471 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -203,12 +203,12 @@ public: int get_mavlink_fd() { return _mavlink_fd; } - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } protected: Mavlink *next; @@ -224,8 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ -- cgit v1.2.3 From dd88e319ee774ac9c8b7726d2fb4dbcc5b04078b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:33:10 +0200 Subject: Reverted logging. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index d6fbe8c4f..c5aef8273 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 1 -t -e + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b9e0663b7..0186cddea 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance -- cgit v1.2.3 From e5105f6d9124dd12a86bec445a8b6f483adfbc56 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:35:30 +0200 Subject: Whtespace. --- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0186cddea..f4a4ce86c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance -- cgit v1.2.3 From 8a946f0320c4bd4a61927a12b7ba4c0c96c77d7d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:37:58 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 180 +++++++++++++++---------------- src/modules/mavlink/mavlink_receiver.cpp | 6 +- 2 files changed, 93 insertions(+), 93 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 37929edac..2d1d92243 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -644,40 +644,40 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); @@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamViconPositionEstimate(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 61ef2b043..b4fe65fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -182,9 +182,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void -- cgit v1.2.3 From e7c8fdc586e59d50579470336157feb14c65ac5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:41:00 +0200 Subject: More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++++++---------------- 1 file changed, 67 insertions(+), 67 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d1d92243..648228e3b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -669,13 +669,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); } } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -- cgit v1.2.3 From 88357c58bde304567f841abc48a495794d4e250c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 20:45:03 +0200 Subject: mavlink: Change to size optimization --- src/modules/mavlink/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadc..c44354ff0 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 80cd2e6c9b03eb078aafe2814ff1c9d2753ac073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:29:12 +0200 Subject: Added fields to range finder clarifying sensor properties --- src/drivers/drv_range_finder.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cf91f7030..e45939b37 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,6 +46,10 @@ #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +enum RANGE_FINDER_TYPE { + RANGE_FINDER_TYPE_LASER = 0, +}; + /** * range finder report structure. Reads from the device must be in multiples of this * structure. @@ -53,8 +57,11 @@ struct range_finder_report { uint64_t timestamp; uint64_t error_count; - float distance; /** in meters */ - uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ + unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ + float distance; /**< in meters */ + float minimum_distance; /**< minimum distance the sensor can measure */ + float maximum_distance; /**< maximum distance the sensor can measure */ + uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; /* -- cgit v1.2.3 From f84669039579aeef80e232c3c9a444d20bcbdf39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:30:09 +0200 Subject: Added rangefinder message to MAVLink app --- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 47 ++++++++++++++++++++++++++++++++ src/modules/mavlink/module.mk | 2 ++ 3 files changed, 50 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f4001..c90b4e2de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1942,6 +1942,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..e608bf787 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -1271,6 +1272,51 @@ protected: } }; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + (void)range_sub->update(t); + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1297,5 +1343,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), nullptr }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadc..dcca11977 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 5c08a1aeaca2c86b66ee1ad05b0178ff214847e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:32:52 +0200 Subject: Untangled local pos and distance messages, now sending distance messages only for actual distance measuring devices --- src/modules/sdlog2/sdlog2.c | 18 +++--------------- src/modules/sdlog2/sdlog2_messages.h | 5 ++++- 2 files changed, 7 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1bf6f1de3..611491cf9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; - /* track changes in distance status */ - bool dist_bottom_present = false; - /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom; + log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate; log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; @@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; + log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); LOGBUFFER_WRITE_AND_COUNT(LPOS); - - if (buf.local_pos.dist_bottom_valid) { - dist_bottom_present = true; - } - - if (dist_bottom_present) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; - log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } } /* --- LOCAL POSITION SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d0585df39..2538dcd2f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -101,6 +101,8 @@ struct log_LPOS_s { float x; float y; float z; + float ground_dist; + float ground_dist_rate; float vx; float vy; float vz; @@ -110,6 +112,7 @@ struct log_LPOS_s { uint8_t xy_flags; uint8_t z_flags; uint8_t landed; + uint8_t ground_dist_flags; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -343,7 +346,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), -- cgit v1.2.3 From 282f40d16247db6ae2598e7f28cd5cc42c4600d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Apr 2014 19:25:07 +0200 Subject: Hotfix to PX4IO uploader. There are no known mishaps due to it, but very clearly the IO firmware flashing process should be verified after an upload. --- src/drivers/px4io/px4io_uploader.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index dd8abbac5..28ec62356 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[]) continue; } - if (bl_rev <= 2) + if (bl_rev <= 2) { ret = verify_rev2(fw_size); - else if(bl_rev == 3) { + } else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } else { + /* verify rev 4 and higher still uses the same approach and + * every version *needs* to be verified. + */ ret = verify_rev3(fw_size); } -- cgit v1.2.3 From 967e9b68783746765a417f613b3ba2935bd6c953 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Wed, 16 Apr 2014 21:41:00 -0700 Subject: Fixed blinkm state indication (was not properly reporting flight mode). Added flashing orange safety disarmed state indicator and solid blue failsafe indicator. Changed safety on state to solid cyan. Increased LiPo cellcount support to 6. --- src/drivers/blinkm/blinkm.cpp | 141 ++++++++++++++++++++++++++++-------------- 1 file changed, 95 insertions(+), 46 deletions(-) (limited to 'src') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 2361f4dd1..b75c2297f 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -48,11 +48,14 @@ * The recognized number off cells, will be blinked 5 times in purple color. * 2 Cells = 2 blinks * ... - * 5 Cells = 5 blinks + * 6 Cells = 6 blinks * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. * - * System disarmed: - * The BlinkM should lit solid red. + * System disarmed and safe: + * The BlinkM should light solid cyan. + * + * System safety off but not armed: + * The BlinkM should light flashing orange * * System armed: * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. @@ -67,10 +70,10 @@ * (X = on, _=off) * * The first 3 blinks indicates the status of the GPS-Signal (red): - * 0-4 satellites = X-X-X-X-_-_-_-_-_-_- - * 5 satellites = X-X-_-X-_-_-_-_-_-_- - * 6 satellites = X-_-_-X-_-_-_-_-_-_- - * >=7 satellites = _-_-_-X-_-_-_-_-_-_- + * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- + * 5 satellites = X-X-_-X-X-_-_-_-_-_- + * 6 satellites = X-_-_-X-X-_-_-_-_-_- + * >=7 satellites = _-_-_-X-X-_-_-_-_-_- * If no GPS is found the first 3 blinks are white * * The fourth Blink indicates the Flightmode: @@ -119,6 +122,7 @@ #include #include #include +#include static const float MAX_CELL_VOLTAGE = 4.3f; static const int LED_ONTIME = 120; @@ -166,10 +170,12 @@ private: enum ledColors { LED_OFF, LED_RED, + LED_ORANGE, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, + LED_CYAN, LED_WHITE, LED_AMBER }; @@ -380,6 +386,7 @@ BlinkM::led() static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; static int actuator_armed_sub_fd; + static int safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -402,16 +409,20 @@ BlinkM::led() if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(vehicle_status_sub_fd, 1000); + orb_set_interval(vehicle_status_sub_fd, 250); vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(vehicle_control_mode_sub_fd, 1000); + orb_set_interval(vehicle_control_mode_sub_fd, 250); actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(actuator_armed_sub_fd, 1000); + orb_set_interval(actuator_armed_sub_fd, 250); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(vehicle_gps_position_sub_fd, 1000); + orb_set_interval(vehicle_gps_position_sub_fd, 250); + + /* Subscribe to safety topic */ + safety_sub_fd = orb_subscribe(ORB_ID(safety)); + orb_set_interval(safety_sub_fd, 250); topic_initialized = true; } @@ -433,7 +444,9 @@ BlinkM::led() if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } - t_led_color[5] = LED_OFF; + if(num_of_cells > 5) { + t_led_color[5] = LED_PURPLE; + } t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; @@ -467,14 +480,17 @@ BlinkM::led() struct vehicle_control_mode_s vehicle_control_mode; struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; + struct safety_s safety; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); + memset(&safety, 0, sizeof(safety)); bool new_data_vehicle_status; bool new_data_vehicle_control_mode; bool new_data_actuator_armed; bool new_data_vehicle_gps_position; + bool new_data_safety; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -520,7 +536,12 @@ BlinkM::led() no_data_vehicle_gps_position = 3; } + /* update safety topic */ + orb_check(safety_sub_fd, &new_data_safety); + if (new_data_safety) { + orb_copy(ORB_ID(safety), safety_sub_fd, &safety); + } /* get number of used satellites in navigation */ num_of_used_sats = 0; @@ -541,19 +562,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* LED Pattern for battery low warning */ - led_color_1 = LED_YELLOW; - led_color_2 = LED_YELLOW; - led_color_3 = LED_YELLOW; - led_color_4 = LED_YELLOW; - led_color_5 = LED_YELLOW; - led_color_6 = LED_YELLOW; - led_color_7 = LED_YELLOW; - led_color_8 = LED_YELLOW; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -565,21 +574,56 @@ BlinkM::led() led_color_8 = LED_RED; led_blink = LED_BLINK; + } else if(vehicle_status_raw.rc_signal_lost) { + /* LED Pattern for FAILSAFE */ + led_color_1 = LED_BLUE; + led_color_2 = LED_BLUE; + led_color_3 = LED_BLUE; + led_color_4 = LED_BLUE; + led_color_5 = LED_BLUE; + led_color_6 = LED_BLUE; + led_color_7 = LED_BLUE; + led_color_8 = LED_BLUE; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + /* LED Pattern for battery low warning */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_color_7 = LED_YELLOW; + led_color_8 = LED_YELLOW; + led_blink = LED_BLINK; + } else { /* no battery warnings here */ if(actuator_armed.armed == false) { /* system not armed */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_NOBLINK; - + if(safety.safety_off){ + led_color_1 = LED_ORANGE; + led_color_2 = LED_ORANGE; + led_color_3 = LED_ORANGE; + led_color_4 = LED_ORANGE; + led_color_5 = LED_ORANGE; + led_color_6 = LED_ORANGE; + led_color_7 = LED_ORANGE; + led_color_8 = LED_ORANGE; + led_blink = LED_BLINK; + }else{ + led_color_1 = LED_CYAN; + led_color_2 = LED_CYAN; + led_color_3 = LED_CYAN; + led_color_4 = LED_CYAN; + led_color_5 = LED_CYAN; + led_color_6 = LED_CYAN; + led_color_7 = LED_CYAN; + led_color_8 = LED_CYAN; + led_blink = LED_NOBLINK; + } } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; @@ -593,23 +637,22 @@ BlinkM::led() led_blink = LED_BLINK; if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - - //XXX please check - if (vehicle_control_mode.flag_control_position_enabled) + /* indicate main control state */ + if (vehicle_status_raw.main_state == MAIN_STATE_EASY) led_color_4 = LED_GREEN; - else if (vehicle_control_mode.flag_control_velocity_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_control_mode.flag_control_attitude_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) led_color_4 = LED_YELLOW; - else if (vehicle_control_mode.flag_control_manual_enabled) - led_color_4 = LED_AMBER; + else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; - + led_color_5 = led_color_4; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat�s */ + /* handling used satus */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; @@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_RED: // red set_rgb(255,0,0); break; + case LED_ORANGE: // orange + set_rgb(255,150,0); + break; case LED_YELLOW: // yellow - set_rgb(255,70,0); + set_rgb(200,200,0); break; case LED_PURPLE: // purple set_rgb(255,0,255); @@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_BLUE: // blue set_rgb(0,0,255); break; + case LED_CYAN: // cyan + set_rgb(0,128,128); + break; case LED_WHITE: // white set_rgb(255,255,255); break; case LED_AMBER: // amber - set_rgb(255,20,0); + set_rgb(255,65,0); break; } } -- cgit v1.2.3 From a4b10bab3042d653c5f59e704cb58008f7551401 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: navigator: wrong mission topic was copied, clearer naming of offboard mission now --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f4a4ce86c..2416a1264 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -832,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a94b6671..bcb3c8d0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9594c4c6a..ef4bc1def 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -105,7 +105,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif -- cgit v1.2.3 From 1f2e972ea683a4d2f33af0e91308f6efed2465c9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Apr 2014 12:16:45 +0200 Subject: mavlink: remaining battery scaling fixed --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 58af1fcb6..678ce1645 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -271,7 +271,7 @@ protected: status->load * 1000.0f, status->battery_voltage * 1000.0f, status->battery_current * 1000.0f, - status->battery_remaining, + status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, status->errors_count1, -- cgit v1.2.3 From 65e2062d7b19f5a95194b574b195efcc4817b388 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 20:56:02 +0200 Subject: sdlog2: fix lpos labels string, shorten messages with excessive length --- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2538dcd2f..affabe125 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,7 +346,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distr,VX,VY,VZ,RLat,RLon,RAlt,XYFlag,ZFlag,LFlag"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -363,8 +363,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_stat,stat_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ -- cgit v1.2.3 From 6297b451ba7dab299576f1d30c565e49b893ba5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 21:00:24 +0200 Subject: sdlog2: Fix indendation to expose length better, cut string lengths for excessive strings --- src/modules/sdlog2/sdlog2_messages.h | 48 ++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index affabe125..21d407d63 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -342,30 +342,30 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distr,VX,VY,VZ,RLat,RLon,RAlt,XYFlag,ZFlag,LFlag"), - LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), - LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), - LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), - LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), - LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), - LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), - LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), - LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_stat,stat_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), - LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), + LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distr,VX,VY,VZ,RLat,RLon,RAlt,XYFlag,ZFlag,LFlag"), + LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), + LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), + LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), + LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + 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"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 5e32ca29d553b5706bbc2eb2f8905d0992ad3570 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 21:25:54 +0200 Subject: Fixed LPOS message in log, added ground flags field --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 21d407d63..28d0706ff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -346,7 +346,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distr,VX,VY,VZ,RLat,RLon,RAlt,XYFlag,ZFlag,LFlag"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), -- cgit v1.2.3 From a0c922704457b540b236f1c72ed5630662da9d9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:38:15 +0200 Subject: commander: Also publish battery status in HIL, since we have a fake battery available and the system freaks out without knowing its main supply --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..819dfbe83 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; -- cgit v1.2.3 From bf4558c31b46bb5d7b30729dd23302048447a890 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Tue, 22 Apr 2014 01:19:01 -0400 Subject: Reduce data manager SD card wear and tear When the data manager was first designed each file record contained a 2 byte header and an 126 byte data section, resulting in a record length of 128 bytes. Along the way it was decided to add 2 spare bytes to the record header, but regrettably the data section was not correspondingly reduced in size so we end up with a record length of 130 bytes. This is bad since it does not align with SD card flash sectors and results in more erase/write flash cycles than necessary thus reducing the SD cards life. This update reduced the data section of the data manager to 124, resulting in an optimal record length of 128 bytes. In order to avoid the reuse of data previously written data in the old format, which could result in catastrophic misinterpretation, the data manager file is checked at startup. If it is found to be in the old format, it is deleted and recreated with in the new record length. In this case previously stored data is lost, but that is far safer than the unpredictable result of using the old file. --- src/modules/dataman/dataman.c | 19 +++++++++++++++++-- src/modules/dataman/dataman.h | 2 +- 2 files changed, 18 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 34d20e485..c132b0040 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -44,7 +44,9 @@ #include #include #include +#include #include +#include #include "dataman.h" @@ -594,6 +596,20 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* See if the data manage file exists and is a multiple of the sector size */ + g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); + if (g_task_fd >= 0) { + /* File exists, check its size */ + int file_size = lseek(g_task_fd, 0, SEEK_END); + if ((file_size % k_sector_size) != 0) { + warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path); + close(g_task_fd); + unlink(k_data_manager_device_path); + } + else + close(g_task_fd); + } + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); @@ -603,7 +619,7 @@ task_main(int argc, char *argv[]) return -1; } - if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { + if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); sem_post(&g_init_sema); /* Don't want to hang startup */ @@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[]) exit(1); } - diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index a70638ccc..33c9fcd15 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -79,7 +79,7 @@ extern "C" { } dm_reset_reason; /* Maximum size in bytes of a single item instance */ - #define DM_MAX_DATA_SIZE 126 + #define DM_MAX_DATA_SIZE 124 /* Retrieve from the data manager store */ __EXPORT ssize_t -- cgit v1.2.3