dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/master' into hott

This commit is contained in:
Simon Wilks 2012-11-21 07:32:59 +01:00
parent 6894c4d1bf 3016ae72a3
commit 4a509684a7
26 changed files with 537 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ROMFS/scripts/rc.sensors
View File

@ -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

64
ROMFS/scripts/rc.standalone
View File

@ -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"

4
apps/ardrone_interface/ardrone_interface.c
View File

@ -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++;
}

43
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -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);
}
}
}

7
apps/commander/commander.c
View File

@ -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;

18
apps/commander/state_machine_helper.c
View File

@ -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]);
}

3
apps/drivers/drv_accel.h
View File

@ -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 */

3
apps/drivers/drv_gyro.h
View File

@ -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 */

6
apps/drivers/drv_led.h
View File

@ -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)

3
apps/drivers/drv_mag.h
View File

@ -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 */

37
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -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)

44
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -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

7
apps/gps/mtk.c
View File

@ -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);

14
apps/gps/ubx.c
View File

@ -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);
}

257
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -65,6 +65,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
@ -104,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -118,7 +120,10 @@ mc_thread_main(int argc, char *argv[])
* rate-limit the attitude subscription to 200Hz to pace our loop
* orb_set_interval(att_sub, 5);
*/
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
struct pollfd fds[2] = {
{ .fd = att_sub, .events = POLLIN },
{ .fd = param_sub, .events = POLLIN }
};
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@ -131,7 +136,9 @@ mc_thread_main(int argc, char *argv[])
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */
printf("[multirotor_att_control] starting\n");
@ -140,143 +147,163 @@ mc_thread_main(int argc, char *argv[])
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
bool flag_system_armed = false;
bool man_yaw_zero_once = false;
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
int ret = poll(fds, 2, 500);
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");

4
apps/multirotor_att_control/multirotor_attitude_control.c
View File

@ -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);
}

8
apps/px4/tests/test_adc.c
View File

@ -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:

8
apps/px4/tests/test_sensors.c
View File

@ -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;

11
apps/sensors/sensors.cpp
View File

@ -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;

42
apps/systemcmds/preflight_check/Makefile Normal file
View File

@ -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

198
apps/systemcmds/preflight_check/preflight_check.c Normal file
View File

@ -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);
}

4
apps/systemlib/param/param.c
View File

@ -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;
}

2
apps/systemlib/param/param.h
View File

@ -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.

7
nuttx/arch/arm/src/stm32/stm32_adc.c
View File

@ -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?
}
/****************************************************************************

1
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -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

1
nuttx/fs/fs_files.c
View File

@ -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;
}