From 63dae04c22009512abe9a0450480698de2bba9db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Apr 2014 17:06:06 +0200 Subject: [PATCH 01/16] 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(-) 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 840cd585ec..8ea6118022 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)) { From b1e4b34a56bfaaa98adb253195d9751f960d5a94 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:58:18 +0200 Subject: [PATCH 02/16] Logging changes, baud rate changes. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273c..d6fbe8c4fa 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 8 -t + sdlog2 start -r 50 -a -b 1 -t -e else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a2..a62f67718b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" # Exit from nsh to free port for mavlink set EXIT_ON_END yes From e99291d825cfc89c67cadd8487e568d3b21d218f Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:37 +0200 Subject: [PATCH 03/16] Added vicon stream. --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b3903..37929edac6 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 }; From 7b85c455558db7971c97c4e8b4d356ebc28b30a1 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:47 +0200 Subject: [PATCH 04/16] Set startup script to do mavlinks. --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a62f67718b..f0bdf32f86 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,8 +421,16 @@ then fi fi + # Main mavlink mavlink start $MAVLINK_FLAGS - + # Optical Flow + mavlink start -d /dev/ttyS3 -m custom -b 115200 + # Android board + #mavlink start -d /dev/ttyS2 -m custom -b 115200 + #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 + #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + # # Start the datamanager # From e6542653b9d013d5cb1b1c0f01ee9af7de4abe5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:15 +0200 Subject: [PATCH 05/16] 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(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f40014..b9e0663b71 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 9941a5f993..2c18261399 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 c66350f5ba..61ef2b043e 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 From 241137d2b9ecc960af3b722d1b0a1a5d9c74e52d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:33 +0200 Subject: [PATCH 06/16] Fixed final parameters in rcS --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0bdf32f86..e37cab04c6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -422,14 +422,14 @@ then fi # Main mavlink - mavlink start $MAVLINK_FLAGS + mavlink start -d /dev/ttyS0 -b 57600 + mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 # Optical Flow mavlink start -d /dev/ttyS3 -m custom -b 115200 # Android board - #mavlink start -d /dev/ttyS2 -m custom -b 115200 - #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 - #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + mavlink start -d /dev/ttyS2 -m custom -b 115200 -w + mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 # # Start the datamanager @@ -453,7 +453,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - gps start + #gps start fi # From 58f471af17adf706ee8e094e9129de43533fb008 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:27:35 +0200 Subject: [PATCH 07/16] Reverted rcS --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e37cab04c6..0cb6d281a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,16 +421,8 @@ then fi fi - # Main mavlink - mavlink start -d /dev/ttyS0 -b 57600 - mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 - # Optical Flow - mavlink start -d /dev/ttyS3 -m custom -b 115200 - # Android board - mavlink start -d /dev/ttyS2 -m custom -b 115200 -w - mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 - + mavlink start $MAVLINK_FLAGS + # # Start the datamanager # @@ -453,7 +445,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - #gps start + gps start fi # From 4f91fdb98ca03f27ea130a53636b4b7990fba183 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:30:00 +0200 Subject: [PATCH 08/16] Indentation. --- src/modules/mavlink/mavlink_main.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2c18261399..66d82b4718 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 */ From dd88e319ee774ac9c8b7726d2fb4dbcc5b04078b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:33:10 +0200 Subject: [PATCH 09/16] Reverted logging. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index d6fbe8c4fa..c5aef8273c 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 b9e0663b71..0186cddea2 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 From e5105f6d9124dd12a86bec445a8b6f483adfbc56 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:35:30 +0200 Subject: [PATCH 10/16] Whtespace. --- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0186cddea2..f4a4ce86c6 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 From 8a946f0320c4bd4a61927a12b7ba4c0c96c77d7d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:37:58 +0200 Subject: [PATCH 11/16] More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 178 +++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 6 +- 2 files changed, 92 insertions(+), 92 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 37929edac6..2d1d92243c 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 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 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 61ef2b043e..b4fe65fd28 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 From e7c8fdc586e59d50579470336157feb14c65ac5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:41:00 +0200 Subject: [PATCH 12/16] More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++------------ 1 file changed, 67 insertions(+), 67 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d1d92243c..648228e3be 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); From 9be0dcdab1820cd5cc6f6fc6a447f8c5a52784cc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Apr 2014 21:44:51 +0200 Subject: [PATCH 13/16] startup: cleanup of cox mixer files (Thanks Rune) --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} | 0 4 files changed, 3 insertions(+), 3 deletions(-) rename ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 38e65435b6..df2e609bc8 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -9,7 +9,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER hexa_cox +set MIXER FMU_hexa_cox # We only can run one channel group with one rate, so set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 99f902a6dd..8703f5f2fe 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER octo_cox +set MIXER FMU_octo_cox set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a2..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -514,7 +514,7 @@ then then set MAV_TYPE 13 fi - if [ $MIXER == hexa_cox ] + if [ $MIXER == FMU_hexa_cox ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/hexa_cox.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix From 282f40d16247db6ae2598e7f28cd5cc42c4600d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Apr 2014 19:25:07 +0200 Subject: [PATCH 14/16] 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(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index dd8abbac5a..28ec62356b 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); } From 967e9b68783746765a417f613b3ba2935bd6c953 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Wed, 16 Apr 2014 21:41:00 -0700 Subject: [PATCH 15/16] 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(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 2361f4dd18..b75c2297fa 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; } } From a4b10bab3042d653c5f59e704cb58008f7551401 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: [PATCH 16/16] 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(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f4a4ce86c6..2416a12640 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 5a94b66717..bcb3c8d0a1 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 c8a589bb7c..90675bb2e9 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 9594c4c6a2..ef4bc1defd 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