From d5a9dffd43b699dfd482fb2ef70677e759eda2af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Apr 2015 13:33:54 +0200 Subject: [PATCH 01/11] Startup script: Add OSD support --- ROMFS/px4fmu_common/init.d/rcS | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 67ec7821c9..e2b247a76c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -461,10 +461,16 @@ then # if ver hwcmp PX4FMU_V2 then + # XXX We need a better way for runtime eval of shell variables, + # but this works for now if param compare SYS_COMPANION 921600 then mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 fi + if param compare SYS_COMPANION 57600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + fi fi # UAVCAN From b7b986359531add925da2eccf34e154851b414db Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 23 Apr 2015 20:27:57 +0200 Subject: [PATCH 02/11] corrected PWM_OUT for firefly6 configuration --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 9489bfd9a2..a20ca04df7 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -8,7 +8,7 @@ sh /etc/init.d/rc.vtol_defaults set MIXER firefly6 -set PWM_OUT 123456 +set PWM_OUT 12345678 set MIXER_AUX firefly6 set PWM_AUX_RATE 50 From 3f961bf3c60c3ea380dc1dc62684a026667ec0e9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 23 Apr 2015 22:02:34 +0300 Subject: [PATCH 03/11] UAVCAN driver silently ignores repeated start commands without error. This allows to avoid error messages when UAVCAN driver is started from extras script before default initialization sequence is executed. --- src/modules/uavcan/uavcan_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2d5abf9591..f04ab9f17c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -698,7 +698,9 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "start")) { if (UavcanNode::instance()) { - errx(1, "already started"); + // Already running, no error + warnx("already started"); + ::exit(0); } // Node ID From c5ce8f39ae6a25655fc7f85a0639458bb170b312 Mon Sep 17 00:00:00 2001 From: andrea-nisti Date: Fri, 24 Apr 2015 15:51:52 +0200 Subject: [PATCH 04/11] Order fixed for vision position quaternion changed from [x y z w] to [w x y z]. In this way the notation is consistent and flightplot shows the real values. --- src/modules/sdlog2/sdlog2.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8193cf1814..e2274342e6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1648,10 +1648,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VISN.vx = buf.vision_pos.vx; log_msg.body.log_VISN.vy = buf.vision_pos.vy; log_msg.body.log_VISN.vz = buf.vision_pos.vz; - log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; - log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; - log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; - log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention + log_msg.body.log_VISN.qx = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[3]; LOGBUFFER_WRITE_AND_COUNT(VISN); } From 1f5775615ffb04f22456795ea39cb1402ae51d5f Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Fri, 24 Apr 2015 16:45:17 +0200 Subject: [PATCH 05/11] Rvis transposed, in this way we have consisency --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7d..68780d5dd7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = Rvis * v; - + math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); From e621b2eb1847aa66c857a16c2046421512fffcc2 Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Fri, 24 Apr 2015 17:03:05 +0200 Subject: [PATCH 06/11] code style fix --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 68780d5dd7..3faf63a27f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -466,7 +466,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw - // Rrw * Vw = vn. This way we have consistency + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); From 5c4494b1c9e9be2c6e88d4078c32f08ef2f12124 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Apr 2015 00:38:03 +0200 Subject: [PATCH 07/11] commander: Fixing HIL operation with failing preflight checks --- src/modules/commander/commander.cpp | 7 ++++--- src/modules/commander/state_machine_helper.cpp | 12 +++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65fc8f90e8..b787a9d4d4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -553,7 +553,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s else { //Refuse to arm if preflight checks have failed - if(!status.condition_system_sensors_initialized) { + if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); cmd_result = VEHICLE_CMD_RESULT_DENIED; break; @@ -1579,7 +1579,9 @@ int commander_thread_main(int argc, char *argv[]) /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + if (armed.armed) { + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); + } status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; @@ -1587,7 +1589,6 @@ int commander_thread_main(int argc, char *argv[]) && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; - mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (!armed.armed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7410baca3e..9aba61e14c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -137,6 +137,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; + prearm_ret = OK; + status->condition_system_sensors_initialized = true; } else { armed->lockdown = false; @@ -174,7 +176,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } @@ -184,7 +186,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { @@ -211,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; @@ -221,9 +223,9 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); } else { - mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } From 75df06bc763f52016ecf7df1d5a43e90d97c13c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:43:47 +0200 Subject: [PATCH 08/11] commander: Better text feedback in preflight-check --- src/modules/commander/PreflightCheck.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 6bb7e5c245..b0a5877628 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -93,7 +93,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); + "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; } @@ -137,7 +137,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); + "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; goto out; } @@ -161,7 +161,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still"); + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); /* this is frickin' fatal */ success = false; goto out; @@ -204,7 +204,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); + "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; goto out; } From af22c49497aac4dda0ea7f941ac27e82dd5471a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 09:45:16 +0200 Subject: [PATCH 09/11] MAVLink app: send correct value when not estimating battery charge level --- src/modules/mavlink/mavlink_messages.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5adfea36b6..276035aa20 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -539,7 +539,8 @@ protected: msg.errors_count2 = status.errors_count2; msg.errors_count3 = status.errors_count3; msg.errors_count4 = status.errors_count4; - msg.battery_remaining = status.battery_remaining * 100.0f; + msg.battery_remaining = (msg.voltage_battery > 0) ? + status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } From 7e8177890835c03de76d75e737d75291c659bbf3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 10:15:15 +0200 Subject: [PATCH 10/11] commander: Fix data link lost / regained logic --- src/modules/commander/commander.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b787a9d4d4..d67f184ced 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1828,13 +1828,17 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { - /* handle the case where data link was regained, + /* handle the case where data link was gained first time or regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - mavlink_log_info(mavlink_fd, "data link %i regained", i); + /* only report a regain */ + if (telemetry_last_dl_loss[i] > 0) { + mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i); + } + telemetry_lost[i] = false; have_link = true; @@ -1845,10 +1849,12 @@ int commander_thread_main(int argc, char *argv[]) } } else { - telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { - mavlink_log_info(mavlink_fd, "data link %i lost", i); + /* only reset the timestamp to a different time on state change */ + telemetry_last_dl_loss[i] = hrt_absolute_time(); + + mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i); telemetry_lost[i] = true; } } @@ -1863,7 +1869,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST"); + mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status.data_link_lost_counter++; status_changed = true; From c7ecafe99b6c6c567d9bd8ac223d5e20592644bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Apr 2015 12:53:58 +0200 Subject: [PATCH 11/11] commander: Critical fix for arm state machine. Only auto-save if not stored already --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d67f184ced..5e03461553 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2763,8 +2763,6 @@ void *commander_low_prio_loop(void *arg) // Update preflight check status status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); @@ -2788,9 +2786,15 @@ void *commander_low_prio_loop(void *arg) } } else if (((int)(cmd.param1)) == 1) { + int ret = param_save_default(); if (ret == OK) { + if (need_param_autosave) { + need_param_autosave = false; + need_param_autosave_timeout = 0; + } + mavlink_log_info(mavlink_fd, "settings saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);