From b5665c2a71c9de50d940a3507cc7cf6cc224757e Mon Sep 17 00:00:00 2001 From: Nick Butcher Date: Mon, 12 Nov 2012 23:18:20 +0100 Subject: [PATCH 01/21] 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 Date: Wed, 14 Nov 2012 09:41:31 +0100 Subject: [PATCH 02/21] 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 Date: Wed, 14 Nov 2012 10:41:44 +0100 Subject: [PATCH 03/21] 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 #include +#include #include #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 Date: Wed, 14 Nov 2012 10:42:02 +0100 Subject: [PATCH 04/21] 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 Date: Wed, 14 Nov 2012 10:42:46 +0100 Subject: [PATCH 05/21] Minor cleanups in attitude control --- .../multirotor_att_control_main.c | 257 ++++++++++-------- .../multirotor_attitude_control.c | 4 +- 2 files changed, 144 insertions(+), 117 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 #include #include +#include #include #include @@ -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); - perf_begin(mc_loop_perf); + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } else if (ret == 0) { + /* no return value, ignore */ + } else { - /* 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); + /* 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 + } + + /* only run controller if attitude changed */ + if (fds[0].revents & POLLIN) { + + 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); + /** 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; } - 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; + /** 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.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); - } + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); - } + float gyro[3]; - /** 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); - } + /* 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; - if (state.flag_control_rates_enabled) { + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - float gyro[3]; + /* 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; - /* 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; - - 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; - - 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 Date: Wed, 14 Nov 2012 13:42:16 +0100 Subject: [PATCH 06/21] 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 Date: Wed, 14 Nov 2012 15:17:06 +0100 Subject: [PATCH 07/21] 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 Date: Wed, 14 Nov 2012 15:17:30 +0100 Subject: [PATCH 08/21] 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 Date: Wed, 14 Nov 2012 15:17:49 +0100 Subject: [PATCH 09/21] 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 Date: Wed, 14 Nov 2012 15:18:16 +0100 Subject: [PATCH 10/21] 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]); } From f803540415f8b4dc7baef98b4a0270d68a33cdcd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 13:21:00 +0100 Subject: [PATCH 11/21] Added preflight_check app which checks core system sensors, so far only mag --- apps/drivers/drv_mag.h | 3 +++ apps/drivers/hmc5883/hmc5883.cpp | 23 ++++++++++++++--------- nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 114bcb6464..9aab995a17 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -108,4 +108,7 @@ ORB_DECLARE(sensor_mag); /** excite strap */ #define MAGIOCEXSTRAP _MAGIOC(6) +/** perform self test and report status */ +#define MAGIOCSELFTEST _MAGIOC(7) + #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index a1587b7830..8f35c484cf 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -647,6 +647,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCEXSTRAP: return set_excitement(arg); + case MAGIOCSELFTEST: + return check_calibration(); + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -1032,24 +1035,24 @@ int HMC5883::check_calibration() if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) && (-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) && (-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) { - /* scale is different from one */ - scale_valid = true; - } else { + /* scale is one */ scale_valid = false; + } else { + scale_valid = true; } if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { - /* offset is different from zero */ - offset_valid = true; - } else { + /* offset is zero */ offset_valid = false; + } else { + offset_valid = true; } if (_calibrated != (offset_valid && scale_valid)) { - warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ", - (offset_valid) ? "" : "offset invalid."); + warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", + (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); /* notify about state change */ struct subsystem_info_s info = { @@ -1059,7 +1062,9 @@ int HMC5883::check_calibration() SUBSYSTEM_TYPE_MAG}; orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info); } - return 0; + + /* return 0 if calibrated, 1 else */ + return (!_calibrated); } int HMC5883::set_excitement(unsigned enable) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 798f57e93c..b03ec1eb37 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -54,6 +54,7 @@ CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/bl_update +CONFIGURED_APPS += systemcmds/preflight_check #CONFIGURED_APPS += systemcmds/calibration # Tutorial code from From 68346fbfeb1603dae6737fae8d21f74659b52823 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 13:21:09 +0100 Subject: [PATCH 12/21] Cleaned up include list --- apps/commander/commander.c | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2b59f709a4..c577fe864b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -63,7 +63,6 @@ #include #include #include -#include #include #include "state_machine_helper.h" #include "systemlib/systemlib.h" From 5020a0a06329c5b5c314df42ac003def7a1a316e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 15:13:27 +0100 Subject: [PATCH 13/21] Addes sensor self test commands --- apps/drivers/drv_accel.h | 3 +++ apps/drivers/drv_gyro.h | 3 +++ apps/drivers/hmc5883/hmc5883.cpp | 6 +++--- apps/drivers/mpu6000/mpu6000.cpp | 30 +++++++++++++++++++++++++++++- 4 files changed, 38 insertions(+), 4 deletions(-) diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 3834795e04..794de584b6 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -115,4 +115,7 @@ ORB_DECLARE(sensor_accel); /** get the current accel measurement range in g */ #define ACCELIOCGRANGE _ACCELIOC(8) +/** get the result of a sensor self-test */ +#define ACCELIOCSELFTEST _ACCELIOC(9) + #endif /* _DRV_ACCEL_H */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 0dbf05c5bc..1d0fef6fcc 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_gyro); /** get the current gyro measurement range in degrees per second */ #define GYROIOCGRANGE _GYROIOC(7) +/** check the status of the sensor */ +#define GYROIOCSELFTEST _GYROIOC(8) + #endif /* _DRV_GYRO_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 8f35c484cf..e443416392 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -1032,9 +1032,9 @@ int HMC5883::check_calibration() { bool scale_valid, offset_valid; - if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) && - (-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) && - (-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) { + if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { /* scale is one */ scale_valid = false; } else { diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 90786886a0..2ac71d89ce 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -260,6 +260,13 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + }; /** @@ -494,6 +501,17 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return ret; } +int +MPU6000::self_test() +{ + if (_reads == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (_reads > 0) ? 0 : 1; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -609,6 +627,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_rad_s = 8.0f * 9.81f; return -EINVAL; + case ACCELIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -656,6 +677,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_m_s2 = xx return -EINVAL; + case GYROIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -813,7 +837,11 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + return; + + /* count measurement */ + _reads++; /* * Convert from big to little endian From c0c72662554e3e887b6d5b2f4f84c4cd83aed9b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 15:13:45 +0100 Subject: [PATCH 14/21] Added preflight_check app --- apps/systemcmds/preflight_check/Makefile | 42 ++++ .../preflight_check/preflight_check.c | 198 ++++++++++++++++++ 2 files changed, 240 insertions(+) create mode 100644 apps/systemcmds/preflight_check/Makefile create mode 100644 apps/systemcmds/preflight_check/preflight_check.c diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile new file mode 100644 index 0000000000..f138e2640b --- /dev/null +++ b/apps/systemcmds/preflight_check/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Reboot command. +# + +APPNAME = preflight_check +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c new file mode 100644 index 0000000000..391eea3a8c --- /dev/null +++ b/apps/systemcmds/preflight_check/preflight_check.c @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 reboot.c + * Tool similar to UNIX reboot command + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int preflight_check_main(int argc, char *argv[]); +static int led_toggle(int leds, int led); +static int led_on(int leds, int led); +static int led_off(int leds, int led); + +int preflight_check_main(int argc, char *argv[]) +{ + bool fail_on_error = false; + + if (argc > 1 && !strcmp(argv[1], "--help")) { + warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); + exit(1); + } + + if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { + fail_on_error = true; + } + + bool system_ok = true; + + int fd; + int ret; + + /* give the system some time to sample the sensors in the background */ + usleep(150000); + + + /* ---- MAG ---- */ + close(fd); + fd = open(MAG_DEVICE_PATH, 0); + if (fd < 0) { + warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret != OK) { + warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); + system_ok = false; + goto system_eval; + } + + /* ---- ACCEL ---- */ + + close(fd); + fd = open(ACCEL_DEVICE_PATH, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + warnx("accel self test failed"); + system_ok = false; + goto system_eval; + } + + /* ---- GYRO ---- */ + + close(fd); + fd = open(GYRO_DEVICE_PATH, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + warnx("gyro self test failed"); + system_ok = false; + goto system_eval; + } + + /* ---- BARO ---- */ + + close(fd); + fd = open(BARO_DEVICE_PATH, 0); + + + +system_eval: + + if (system_ok) { + /* all good, exit silently */ + exit(0); + } else { + fflush(stdout); + + int buzzer = open("/dev/tone_alarm", O_WRONLY); + int leds = open(LED_DEVICE_PATH, 0); + + /* flip blue led into alternating amber */ + led_off(leds, LED_BLUE); + led_off(leds, LED_AMBER); + led_toggle(leds, LED_BLUE); + + /* display and sound error */ + for (int i = 0; i < 200; i++) + { + led_toggle(leds, LED_BLUE); + led_toggle(leds, LED_AMBER); + + if (i % 10 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 4); + } else if (i % 5 == 0) { + ioctl(buzzer, TONE_SET_ALARM, 2); + } + usleep(100000); + } + + /* stop alarm */ + ioctl(buzzer, TONE_SET_ALARM, 0); + + /* switch on leds */ + led_on(leds, LED_BLUE); + led_on(leds, LED_AMBER); + + if (fail_on_error) { + /* exit with error message */ + exit(1); + } else { + /* do not emit an error code to make sure system still boots */ + exit(0); + } + } +} + +static int led_toggle(int leds, int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_off(int leds, int led) +{ + return ioctl(leds, LED_OFF, led); +} + +static int led_on(int leds, int led) +{ + return ioctl(leds, LED_ON, led); +} \ No newline at end of file From 1e213ea53c1745aba91db29e29a191d72e8e1810 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 15:14:28 +0100 Subject: [PATCH 15/21] Fixed sensors test --- apps/px4/tests/test_sensors.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index dc1f39046b..14503276cf 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -125,7 +125,7 @@ accel(int argc, char *argv[]) /* read data - expect samples */ ret = read(fd, &buf, sizeof(buf)); - if (ret < 3) { + if (ret != sizeof(buf)) { printf("\tACCEL: read1 fail (%d)\n", ret); return ERROR; @@ -177,7 +177,7 @@ gyro(int argc, char *argv[]) /* read data - expect samples */ ret = read(fd, &buf, sizeof(buf)); - if (ret < 3) { + if (ret != sizeof(buf)) { printf("\tGYRO: read fail (%d)\n", ret); return ERROR; @@ -214,7 +214,7 @@ mag(int argc, char *argv[]) /* read data - expect samples */ ret = read(fd, &buf, sizeof(buf)); - if (ret < 3) { + if (ret != sizeof(buf)) { printf("\tMAG: read fail (%d)\n", ret); return ERROR; @@ -251,7 +251,7 @@ baro(int argc, char *argv[]) /* read data - expect samples */ ret = read(fd, &buf, sizeof(buf)); - if (ret < 3) { + if (ret != sizeof(buf)) { printf("\tBARO: read fail (%d)\n", ret); return ERROR; From dad1304503f301a0a8ab0441d1eeab8638290376 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 15:14:57 +0100 Subject: [PATCH 16/21] Cleaning up start scripts, enabling preflight check as default --- ROMFS/scripts/rc.sensors | 25 +++++++++------ ROMFS/scripts/rc.standalone | 64 ------------------------------------- 2 files changed, 15 insertions(+), 74 deletions(-) diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index f913e82af9..536fcfd2c0 100644 --- a/ROMFS/scripts/rc.sensors +++ b/ROMFS/scripts/rc.sensors @@ -8,21 +8,26 @@ # ms5611 start -mpu6000 start -hmc5883 start + +if mpu6000 start +then + echo "using MPU6000 and HMC5883L" + hmc5883 start +else + echo "using L3GD20 and LSM303D" + l3gd20 start + lsm303 start +fi # # Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. # sensors start # -# Test sensor functionality +# Check sensors - run AFTER 'sensors start' # -# XXX integrate with 'sensors start' ? -# -#if sensors quicktest -#then -# echo "[init] sensor initialisation FAILED." -# reboot -#fi +preflight_check \ No newline at end of file diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/scripts/rc.standalone index 8ccdb577b5..67e95215b9 100644 --- a/ROMFS/scripts/rc.standalone +++ b/ROMFS/scripts/rc.standalone @@ -10,68 +10,4 @@ echo "[init] doing standalone PX4FMU startup..." # uorb start -# -# Init the EEPROM -# -echo "[init] eeprom" -eeprom start -if [ -f /eeprom/parameters ] -then - param load -fi - -# -# Start the sensors. -# -#sh /etc/init.d/rc.sensors - -# -# Start MAVLink -# -# mavlink -d /dev/ttyS0 -b 57600 & - -# -# Start the commander. -# -# XXX this should be 'commander start'. -# -#commander & - -# -# Start the attitude estimator -# -# XXX this should be ' start'. -# -#attitude_estimator_bm & -#position_estimator & - -# -# Start the fixed-wing controller. -# -# XXX should this be looking for configuration to decide -# whether the board is configured for fixed-wing use? -# -# XXX this should be 'fixedwing_control start'. -# -#fixedwing_control & - -# -# Configure FMU for standalone mode -# -# XXX arguments? -# -#px4fmu start - -# -# Start looking for a GPS. -# -# XXX this should not need to be backgrounded -# -#gps -d /dev/ttyS3 -m all & - -# -# Start logging to microSD if we can -# -sh /etc/init.d/rc.logging - echo "[init] startup done" From 1306008467f47f3c6192ff4ed68b98ad9ea7a07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 17:13:32 +0100 Subject: [PATCH 17/21] Fixed NuttX issue causing an assertion to trigger on unlinking / opening files --- nuttx/fs/fs_files.c | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx/fs/fs_files.c b/nuttx/fs/fs_files.c index 425e7c73f8..4da2d28a52 100644 --- a/nuttx/fs/fs_files.c +++ b/nuttx/fs/fs_files.c @@ -400,6 +400,7 @@ int files_allocate(FAR struct inode *inode, int oflags, off_t pos, int minfd) list->fl_files[i].f_oflags = oflags; list->fl_files[i].f_pos = pos; list->fl_files[i].f_inode = inode; + list->fl_files[i].f_priv = NULL; _files_semgive(list); return i; } From 74d543cfc9e2d63caf2d10b4a93227608a2c2930 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 17:14:24 +0100 Subject: [PATCH 18/21] Made u-blox timeouts more forgiving --- apps/gps/ubx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 2bbecb12e1..e15ed4e00a 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -49,7 +49,7 @@ #include #define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2 -#define UBX_HEALTH_FAIL_COUNTER_LIMIT 2 +#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3 #define UBX_HEALTH_PROBE_COUNTER_LIMIT 4 #define UBX_BUFFER_SIZE 1000 From df5e4d19042985bd567845dfa464170c169829b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 17:19:21 +0100 Subject: [PATCH 19/21] Improved self-test logic --- apps/drivers/hmc5883/hmc5883.cpp | 4 +++- apps/drivers/mpu6000/mpu6000.cpp | 14 +++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index e443416392..3849a2e052 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -634,7 +634,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); - return check_calibration(); + /* check calibration, but not actually return an error */ + (void)check_calibration(); + return 0; case MAGIOCGSCALE: /* copy out scale factors */ diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 2ac71d89ce..ed79440ccd 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -610,9 +610,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case ACCELIOCSSCALE: - /* copy scale in */ - memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); - return OK; + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } case ACCELIOCGSCALE: /* copy scale out */ From 7f916779df3d758ade6836adb6e5f66b5b41bdd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 17:19:52 +0100 Subject: [PATCH 20/21] Minor cleanup of param load / store --- apps/systemlib/param/param.c | 4 +--- apps/systemlib/param/param.h | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index ddf9b0975d..ebb72ca3e1 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -517,13 +517,11 @@ param_save_default(void) } int result = param_export(fd, false); - /* should not be necessary, over-careful here */ - fsync(fd); close(fd); if (result != 0) { - unlink(param_get_default_file()); warn("error exporting parameters to '%s'", param_get_default_file()); + unlink(param_get_default_file()); return -2; } diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index 6fa73b5a48..084cd931a3 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -250,7 +250,7 @@ __EXPORT int param_set_default_file(const char* filename); * a result of a call to param_set_default_file, or the * built-in default. */ -__EXPORT const char *param_get_default_file(void); +__EXPORT const char* param_get_default_file(void); /** * Save parameters to the default file. From 3016ae72a3b3b7d7bf1df937fd62a14f53eace6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Nov 2012 17:20:14 +0100 Subject: [PATCH 21/21] minor cosmetic changes in commander --- apps/commander/commander.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c577fe864b..c3e825a86e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -268,6 +268,7 @@ void tune_confirm(void) { void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { + /* set to mag calibration mode */ status->flag_preflight_mag_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -324,7 +325,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) uint64_t axis_deadline = hrt_absolute_time(); uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - const char axislabels[3] = { 'X', 'Z', 'Y'}; + const char axislabels[3] = { 'X', 'Y', 'Z'}; int axis_index = -1; float *x = (float*)malloc(sizeof(float) * calibration_maxcount); @@ -470,6 +471,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) int save_ret = param_save_default(); if(save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "[cmd] FAILED storing calibration"); } printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -1131,7 +1133,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 4096, + 4000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true;