diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-15 11:55:55 -0800 |
commit | 33e750602ab384069b08ca17ca6589c08177f7a6 (patch) | |
tree | 8f2dbfe7779fc74436ea4ba243e2929c09d5b2ac | |
parent | b7c6a11e6739d217e5df1e79b7f80399ff1fd8f8 (diff) | |
parent | 3016ae72a3b3b7d7bf1df937fd62a14f53eace6f (diff) | |
download | px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.gz px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.tar.bz2 px4-firmware-33e750602ab384069b08ca17ca6589c08177f7a6.zip |
Merge remote-tracking branch 'upstream/master' into io
Fixed Conflicts:
apps/multirotor_att_control/multirotor_att_control_main.c
rc loss failsafe throttle tested
26 files changed, 560 insertions, 323 deletions
diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index f913e82af..536fcfd2c 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 -# -# XXX integrate with 'sensors start' ? +# Check sensors - run AFTER '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 8ccdb577b..67e95215b 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 '<command> 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" diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f96d901fb..aeaf830de 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++; } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index b25e61229..6533390bc 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); } } } diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2b59f709a..c3e825a86 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -63,7 +63,6 @@ #include <string.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> -#include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> #include "state_machine_helper.h" #include "systemlib/systemlib.h" @@ -269,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); @@ -325,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); @@ -471,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", @@ -1132,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; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 9c7db8fca..bf50ebad2 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]); } diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 3834795e0..794de584b 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 0dbf05c5b..1d0fef6fc 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/drv_led.h b/apps/drivers/drv_led.h index 7eb9e4b7c..21044f620 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) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 114bcb646..9aab995a1 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 81bc8954b..3849a2e05 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -634,6 +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)); + /* check calibration, but not actually return an error */ (void)check_calibration(); return 0; @@ -648,6 +649,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); @@ -1012,7 +1016,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."); @@ -1025,27 +1034,27 @@ 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)) { - /* scale is different from one */ - scale_valid = true; - } else { + 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 { + 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 = { @@ -1055,7 +1064,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/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 90786886a..ed79440cc 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) { @@ -592,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 */ @@ -609,6 +635,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 +685,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 +845,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 diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 604dba05c..7ba4f52b0 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 21e917bf8..e15ed4e00 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -49,7 +49,7 @@ #include <mavlink/mavlink_log.h> #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 @@ -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); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index de3a4440b..d2de1a787 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> @@ -107,6 +108,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)); @@ -121,7 +123,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++) { @@ -134,7 +139,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"); @@ -143,7 +150,6 @@ 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; /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); @@ -152,180 +158,185 @@ mc_thread_main(int argc, char *argv[]) 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; - if (state.offboard_control_signal_lost) { - /* give up in rate mode on signal loss */ - rates_sp.thrust = 0.0f; - } else { - /* no signal loss, normal throttle */ - rates_sp.thrust = 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 */ } -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); + 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; - - /* check if control signal is lost */ - if (state.offboard_control_signal_lost) { - /* set failsafe thrust */ + } + + 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; + } + + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if(state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; att_sp.thrust = failsafe_throttle; + + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + // XXX hard-limit it to prevent ballistic mishaps - this is just supposed to + // slow a crash down, not actually keep the system in-air. + + rc_loss_first_time = false; + } else { - /* no signal loss, normal throttle */ - att_sp.thrust = offboard_sp.p4; + rc_loss_first_time = true; + + 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(); } - -// printf("thrust_att=%8.4f\n",offboard_sp.p4); + } + /* 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; - - /* 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; - - // XXX disable this for now until its better checked - - static bool rc_loss_first_time = true; - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - // XXX hard-limit it to prevent ballistic mishaps - this is just supposed to - // slow a crash down, not actually keep the system in-air. - att_sp.thrust = failsafe_throttle; - rc_loss_first_time = false; - } else { - rc_loss_first_time = true; - } - - 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); - } - + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); - if (state.flag_control_rates_enabled) { + float gyro[3]; - 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); - } + /* 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 839d56d29..8ffa90238 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); } diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index c33af1311..c5960e757 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: diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index dc1f39046..14503276c 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; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 7c1503f0d..eea51cc1e 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; diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile new file mode 100644 index 000000000..f138e2640 --- /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 000000000..391eea3a8 --- /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 <lm@inf.ethz.ch> + * + * 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 <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <fcntl.h> +#include <errno.h> + +#include <systemlib/err.h> + +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_baro.h> + +__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 diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index ddf9b0975..ebb72ca3e 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 6fa73b5a4..084cd931a 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. diff --git a/nuttx/arch/arm/src/stm32/stm32_adc.c b/nuttx/arch/arm/src/stm32/stm32_adc.c index a45b732ae..844f1fd0f 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? } /**************************************************************************** diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 798f57e93..b03ec1eb3 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 diff --git a/nuttx/fs/fs_files.c b/nuttx/fs/fs_files.c index 425e7c73f..4da2d28a5 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; } |