From b5665c2a71c9de50d940a3507cc7cf6cc224757e Mon Sep 17 00:00:00 2001
From: Nick Butcher <butcher.nick@gmail.com>
Date: Mon, 12 Nov 2012 23:18:20 +0100
Subject: [PATCH 01/10] GPS watchdog - health detection fixes
---
apps/gps/mtk.c | 7 +++----
apps/gps/ubx.c | 12 ++++--------
2 files changed, 7 insertions(+), 12 deletions(-)
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 604dba05c7..7ba4f52b0a 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -410,6 +410,8 @@ void *mtk_watchdog_loop(void *args)
} else {
/* gps healthy */
mtk_success_count++;
+ mtk_fail_count = 0;
+ once_ok = true; // XXX Should this be true on a single success, or on same criteria as mtk_healthy?
if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) {
printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed);
@@ -418,11 +420,8 @@ void *mtk_watchdog_loop(void *args)
mtk_gps->satellite_info_available = 0;
// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
+ mtk_healthy = true;
}
-
- mtk_healthy = true;
- mtk_fail_count = 0;
- once_ok = true;
}
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 21e917bf88..2bbecb12e1 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -786,22 +786,18 @@ void *ubx_watchdog_loop(void *args)
sleep(1);
} else {
+ /* gps healthy */
+ ubx_success_count++;
+ ubx_fail_count = 0;
+ once_ok = true; // XXX Should this be true on a single success, or on same criteria as ubx_healthy?
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
ubx_healthy = true;
- ubx_fail_count = 0;
- once_ok = true;
}
-
- /* gps healthy */
- ubx_success_count++;
- ubx_healthy = true;
- ubx_fail_count = 0;
}
-
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
From 47bf4386615636bd23226ae478291ec5e2dcb1d9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 09:41:31 +0100
Subject: [PATCH 02/10] Fixed ADC shutdown issue
---
nuttx/arch/arm/src/stm32/stm32_adc.c | 7 ++-----
1 file changed, 2 insertions(+), 5 deletions(-)
diff --git a/nuttx/arch/arm/src/stm32/stm32_adc.c b/nuttx/arch/arm/src/stm32/stm32_adc.c
index a45b732aea..844f1fd0fa 100644
--- a/nuttx/arch/arm/src/stm32/stm32_adc.c
+++ b/nuttx/arch/arm/src/stm32/stm32_adc.c
@@ -1427,15 +1427,12 @@ static void adc_shutdown(FAR struct adc_dev_s *dev)
/* power down ADC */
adc_enable(priv, false);
- adc_rxint(priv, false);
+ adc_rxint(dev, false);
/* Disable ADC interrupts and detach the ADC interrupt handler */
up_disable_irq(priv->irq);
// irq_detach(priv->irq);
-
-
-
-
+ // XXX Why is irq_detach here commented out?
}
/****************************************************************************
From 01eed6e946407ca6299a179e3d517ec2631ee9c5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 10:41:44 +0100
Subject: [PATCH 03/10] Added perf counter, cleaned up
---
.../attitude_estimator_ekf_main.c | 43 ++++---------------
1 file changed, 8 insertions(+), 35 deletions(-)
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index b25e612297..6533390bc0 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -63,6 +63,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include "codegen/attitudeKalmanfilter_initialize.h"
@@ -255,6 +256,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
+ /* register the perf counter */
+ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
+
/* Main loop*/
while (!thread_should_exit) {
@@ -306,6 +310,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
} else {
+ perf_begin(ekf_loop_perf);
+
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
@@ -400,41 +406,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
-
- // uint64_t timing_diff = hrt_absolute_time() - timing_start;
-
- // /* print rotation matrix every 200th time */
- if (printcounter % 200 == 0) {
- // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
- // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
- // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
- // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
-
-
- // }
-
- //printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
- //printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
- //printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
- // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
- // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
- // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
- }
-
- // int i = printcounter % 9;
-
- // // for (int i = 0; i < 9; i++) {
- // char name[10];
- // sprintf(name, "xapo #%d", i);
- // memcpy(dbg.key, name, sizeof(dbg.key));
- // dbg.value = x_aposteriori[i];
- // if (pub_dbg < 0) {
- // pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
- // } else {
- // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
- // }
-
- printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
@@ -463,6 +434,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}
+
+ perf_end(ekf_loop_perf);
}
}
}
From d9d002f3aa1e25637538e713189fce565e4c8901 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 10:42:02 +0100
Subject: [PATCH 04/10] Fixed line breaks in ADC test
---
apps/px4/tests/test_adc.c | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c
index c33af1311d..c5960e7570 100644
--- a/apps/px4/tests/test_adc.c
+++ b/apps/px4/tests/test_adc.c
@@ -127,18 +127,18 @@ int test_adc(int argc, char *argv[])
message("channel: %d value: %d\n",
(int)sample1.am_channel1, sample1.am_data1);
- message("channel: %d value: %d",
+ message("channel: %d value: %d\n",
(int)sample1.am_channel2, sample1.am_data2);
- message("channel: %d value: %d",
+ message("channel: %d value: %d\n",
(int)sample1.am_channel3, sample1.am_data3);
- message("channel: %d value: %d",
+ message("channel: %d value: %d\n",
(int)sample1.am_channel4, sample1.am_data4);
}
}
fflush(stdout);
}
- message("\t ADC test successful.");
+ message("\t ADC test successful.\n");
errout_with_dev:
From c2abe3997c730c183a7a5a094cadf1575951f86c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 10:42:46 +0100
Subject: [PATCH 05/10] Minor cleanups in attitude control
---
.../multirotor_att_control_main.c | 261 ++++++++++--------
.../multirotor_attitude_control.c | 4 +-
2 files changed, 146 insertions(+), 119 deletions(-)
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 29463d7447..fd29e36606 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -65,6 +65,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
@@ -104,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -118,7 +120,10 @@ mc_thread_main(int argc, char *argv[])
* rate-limit the attitude subscription to 200Hz to pace our loop
* orb_set_interval(att_sub, 5);
*/
- struct pollfd fds = { .fd = att_sub, .events = POLLIN };
+ struct pollfd fds[2] = {
+ { .fd = att_sub, .events = POLLIN },
+ { .fd = param_sub, .events = POLLIN }
+ };
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -131,7 +136,9 @@ mc_thread_main(int argc, char *argv[])
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */
- perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
+ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
+ perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
+ perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */
printf("[multirotor_att_control] starting\n");
@@ -140,143 +147,163 @@ mc_thread_main(int argc, char *argv[])
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
- bool man_yaw_zero_once = false;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
- poll(&fds, 1, 500);
+ int ret = poll(fds, 2, 500);
+
+ if (ret < 0) {
+ /* poll error, count it in perf */
+ perf_count(mc_err_perf);
+ } else if (ret == 0) {
+ /* no return value, ignore */
+ } else {
+
+ /* only update parameters if they changed */
+ if (fds[1].revents & POLLIN) {
+ /* read from param to clear updated flag */
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+ /* update parameters */
+ // XXX no params here yet
+ }
- perf_begin(mc_loop_perf);
+ /* only run controller if attitude changed */
+ if (fds[0].revents & POLLIN) {
- /* get a local copy of system state */
- bool updated;
- orb_check(state_sub, &updated);
- if (updated) {
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
- }
- /* get a local copy of manual setpoint */
- orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
- /* get a local copy of attitude */
- orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
- /* get a local copy of attitude setpoint */
- orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
- /* get a local copy of rates setpoint */
- orb_check(setpoint_sub, &updated);
- if (updated) {
- orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
- }
- /* get a local copy of the current sensor values */
- orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-
- /** STEP 1: Define which input is the dominating control input */
- if (state.flag_control_offboard_enabled) {
- /* offboard inputs */
- if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
- rates_sp.roll = offboard_sp.p1;
- rates_sp.pitch = offboard_sp.p2;
- rates_sp.yaw = offboard_sp.p3;
- rates_sp.thrust = offboard_sp.p4;
-// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+ perf_begin(mc_loop_perf);
+
+ /* get a local copy of system state */
+ bool updated;
+ orb_check(state_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+ }
+ /* get a local copy of manual setpoint */
+ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+ /* get a local copy of attitude */
+ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+ /* get a local copy of attitude setpoint */
+ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+ /* get a local copy of rates setpoint */
+ orb_check(setpoint_sub, &updated);
+ if (updated) {
+ orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
+ }
+ /* get a local copy of the current sensor values */
+ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+
+ /** STEP 1: Define which input is the dominating control input */
+ if (state.flag_control_offboard_enabled) {
+ /* offboard inputs */
+ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+ rates_sp.roll = offboard_sp.p1;
+ rates_sp.pitch = offboard_sp.p2;
+ rates_sp.yaw = offboard_sp.p3;
+ rates_sp.thrust = offboard_sp.p4;
+ // printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+ rates_sp.timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+ } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+ att_sp.roll_body = offboard_sp.p1;
+ att_sp.pitch_body = offboard_sp.p2;
+ att_sp.yaw_body = offboard_sp.p3;
+ att_sp.thrust = offboard_sp.p4;
+ // printf("thrust_att=%8.4f\n",offboard_sp.p4);
+ att_sp.timestamp = hrt_absolute_time();
+ /* STEP 2: publish the result to the vehicle actuators */
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+ }
+
+ /* decide wether we want rate or position input */
+ }
+ else if (state.flag_control_manual_enabled) {
+ /* manual inputs, from RC control or joystick */
+
+ if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
+ rates_sp.roll = manual.roll;
+
+ rates_sp.pitch = manual.pitch;
+ rates_sp.yaw = manual.yaw;
+ rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
- orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
- } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
- att_sp.roll_body = offboard_sp.p1;
- att_sp.pitch_body = offboard_sp.p2;
- att_sp.yaw_body = offboard_sp.p3;
- att_sp.thrust = offboard_sp.p4;
-// printf("thrust_att=%8.4f\n",offboard_sp.p4);
+ }
+
+ if (state.flag_control_attitude_enabled) {
+
+ /* initialize to current yaw if switching to manual or att control */
+ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
+ state.flag_control_manual_enabled != flag_control_manual_enabled ||
+ state.flag_system_armed != flag_system_armed) {
+ att_sp.yaw_body = att.yaw;
+ }
+
+ att_sp.roll_body = manual.roll;
+ att_sp.pitch_body = manual.pitch;
+
+ /* only move setpoint if manual input is != 0 */
+ // XXX turn into param
+ if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+ att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
+ } else if (manual.throttle <= 0.3f) {
+ att_sp.yaw_body = att.yaw;
+ }
+ att_sp.thrust = manual.throttle;
+ att_sp.timestamp = hrt_absolute_time();
+ }
+ /* STEP 2: publish the result to the vehicle actuators */
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
+ if (motor_test_mode) {
+ printf("testmode");
+ att_sp.roll_body = 0.0f;
+ att_sp.pitch_body = 0.0f;
+ att_sp.yaw_body = 0.0f;
+ att_sp.thrust = 0.1f;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
- /* decide wether we want rate or position input */
}
- else if (state.flag_control_manual_enabled) {
- /* manual inputs, from RC control or joystick */
-
- if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
- rates_sp.roll = manual.roll;
-
- rates_sp.pitch = manual.pitch;
- rates_sp.yaw = manual.yaw;
- rates_sp.thrust = manual.throttle;
- rates_sp.timestamp = hrt_absolute_time();
- }
-
- if (state.flag_control_attitude_enabled) {
- /* initialize to current yaw if switching to manual or att control */
- if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
- state.flag_control_manual_enabled != flag_control_manual_enabled ||
- state.flag_system_armed != flag_system_armed) {
- att_sp.yaw_body = att.yaw;
+ /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
+ if (state.flag_control_attitude_enabled) {
+ multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
- att_sp.roll_body = manual.roll;
- att_sp.pitch_body = manual.pitch;
+ /* measure in what intervals the controller runs */
+ perf_count(mc_interval_perf);
- /* only move setpoint if manual input is != 0 */
- // XXX turn into param
- if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
- att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
- } else if (manual.throttle <= 0.3f) {
- att_sp.yaw_body = att.yaw;
- }
- att_sp.thrust = manual.throttle;
- att_sp.timestamp = hrt_absolute_time();
- }
- /* STEP 2: publish the result to the vehicle actuators */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
- if (motor_test_mode) {
- printf("testmode");
- att_sp.roll_body = 0.0f;
- att_sp.pitch_body = 0.0f;
- att_sp.yaw_body = 0.0f;
- att_sp.thrust = 0.1f;
- att_sp.timestamp = hrt_absolute_time();
- /* STEP 2: publish the result to the vehicle actuators */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
- }
-
- }
-
- /** STEP 3: Identify the controller setup to run and set up the inputs correctly */
- if (state.flag_control_attitude_enabled) {
- multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
- orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
- }
+ float gyro[3];
+ /* get current rate setpoint */
+ bool rates_sp_valid = false;
+ orb_check(rates_sp_sub, &rates_sp_valid);
+ if (rates_sp_valid) {
+ orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+ }
- if (state.flag_control_rates_enabled) {
-
- float gyro[3];
-
- /* get current rate setpoint */
- bool rates_sp_valid = false;
- orb_check(rates_sp_sub, &rates_sp_valid);
- if (rates_sp_valid) {
- orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
- }
-
- /* apply controller */
- gyro[0] = att.rollspeed;
- gyro[1] = att.pitchspeed;
- gyro[2] = att.yawspeed;
+ /* apply controller */
+ gyro[0] = att.rollspeed;
+ gyro[1] = att.pitchspeed;
+ gyro[2] = att.yawspeed;
- multirotor_control_rates(&rates_sp, gyro, &actuators);
- orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
- }
+ multirotor_control_rates(&rates_sp, gyro, &actuators);
+ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
- /* update state */
- flag_control_attitude_enabled = state.flag_control_attitude_enabled;
- flag_control_manual_enabled = state.flag_control_manual_enabled;
- flag_system_armed = state.flag_system_armed;
+ /* update state */
+ flag_control_attitude_enabled = state.flag_control_attitude_enabled;
+ flag_control_manual_enabled = state.flag_control_manual_enabled;
+ flag_system_armed = state.flag_system_armed;
- perf_end(mc_loop_perf);
+ perf_end(mc_loop_perf);
+ } /* end of poll call for attitude updates */
+ } /* end of poll return value check */
}
printf("[multirotor att control] stopping, disarming motors.\n");
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 839d56d29d..8ffa902387 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -199,9 +199,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */
parameters_update(&h, &p);
- /* apply parameters */
- printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+ //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+ /* apply parameters */
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
From ffac5cba2fb91d30334c36b296a982686fcd166a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 13:42:16 +0100
Subject: [PATCH 06/10] Requiring at least four channels for a successful PPM
frame
---
apps/sensors/sensors.cpp | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 7c1503f0d2..eea51cc1eb 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -870,7 +870,12 @@ Sensors::ppm_poll()
/* we are accepting this message */
_ppm_last_valid = ppm_last_valid_decode;
- if (ppm_decoded_channels > 2 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
+ /*
+ * relying on two decoded channels is very noise-prone,
+ * in particular if nothing is connected to the pins.
+ * requiring a minimum of four channels
+ */
+ if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
for (int i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
@@ -898,8 +903,8 @@ Sensors::ppm_poll()
struct manual_control_setpoint_s manual_control;
- /* require at least two chanels to consider the signal valid */
- if (rc_input.channel_count < 2)
+ /* require at least four channels to consider the signal valid */
+ if (rc_input.channel_count < 4)
return;
unsigned channel_limit = rc_input.channel_count;
From aeea27d16a6e7d92f7caf1c6a272a5f4bfa9a721 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 15:17:06 +0100
Subject: [PATCH 07/10] Increased interface slightly to better match 200 Hz,
adjusted led flash speed
---
apps/ardrone_interface/ardrone_interface.c | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index f96d901fbc..aeaf830def 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -340,7 +340,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
}
}
- if (counter % 16 == 0) {
+ if (counter % 24 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
@@ -371,7 +371,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
}
/* run at approximately 200 Hz */
- usleep(5000);
+ usleep(4500);
counter++;
}
From 722af669fea5dcb6719438326f8272cbb8ca69ee Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 15:17:30 +0100
Subject: [PATCH 08/10] Better integrate calibration check
---
apps/drivers/hmc5883/hmc5883.cpp | 10 +++++++---
1 file changed, 7 insertions(+), 3 deletions(-)
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 81bc8954b9..a1587b7830 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -634,8 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
- (void)check_calibration();
- return 0;
+ return check_calibration();
case MAGIOCGSCALE:
/* copy out scale factors */
@@ -1012,7 +1011,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (ret == OK) {
- warnx("mag scale calibration successfully finished.");
+ if (!check_calibration()) {
+ warnx("mag scale calibration successfully finished.");
+ } else {
+ warnx("mag scale calibration finished with invalid results.");
+ ret == ERROR;
+ }
} else {
warnx("mag scale calibration failed.");
From 3eb36bbd2145a2932e5169e1ae6b676bf636debe Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 15:17:49 +0100
Subject: [PATCH 09/10] Fix led assignment for FMU
---
apps/drivers/drv_led.h | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h
index 7eb9e4b7c2..21044f6200 100644
--- a/apps/drivers/drv_led.h
+++ b/apps/drivers/drv_led.h
@@ -47,9 +47,9 @@
#define _LED_BASE 0x2800
/* PX4 LED colour codes */
-#define LED_AMBER 0
-#define LED_RED 0 /* some boards have red rather than amber */
-#define LED_BLUE 1
+#define LED_AMBER 1
+#define LED_RED 1 /* some boards have red rather than amber */
+#define LED_BLUE 0
#define LED_SAFETY 2
#define LED_ON _IOC(_LED_BASE, 0)
From c4bf3ea3ed81acdce9f972534c56e2dc68135d19 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 14 Nov 2012 15:18:16 +0100
Subject: [PATCH 10/10] better system status reporting, work in progress
---
apps/commander/state_machine_helper.c | 18 ++++++++++++++++++
1 file changed, 18 insertions(+)
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 891efe9d79..657c9af9af 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -213,6 +213,24 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/* publish the new state */
current_status->counter++;
current_status->timestamp = hrt_absolute_time();
+
+ /* assemble state vector based on flag values */
+ if (current_status->flag_control_rates_enabled) {
+ current_status->onboard_control_sensors_present |= 0x400;
+ } else {
+ current_status->onboard_control_sensors_present &= ~0x400;
+ }
+ current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+ current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+ current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+ current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
+ current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
+ current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+ current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+ current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+ current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
}