fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 10:04:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/px4io-i2c' into new_state_machine

Browse Source
This commit is contained in:
Julian Oes 2013-02-22 15:53:07 -08:00
commit f731b6f4e5
15 changed files with 229 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
apps/commander/commander.c
View File

@ -429,7 +429,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
// mavlink_log_info(mavlink_fd, buf);
// }
if (poll(fds, 1, 1000)) {
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
@ -461,9 +463,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++;
} else {
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled");
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break;
}
}
@ -553,7 +555,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN)");
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
}
/* disable calibration mode */
@ -598,14 +600,16 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
if (poll(fds, 1, 1000)) {
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
} else {
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
return;
@ -706,14 +710,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
if (poll(fds, 1, 1000)) {
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
accel_offset[0] += raw.accelerometer_m_s2[0];
accel_offset[1] += raw.accelerometer_m_s2[1];
accel_offset[2] += raw.accelerometer_m_s2[2];
calibration_counter++;
} else {
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
return;
@ -1253,11 +1259,10 @@ int commander_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = task_spawn("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 50,
4000,
SCHED_PRIORITY_MAX - 40,
3000,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
@ -1355,7 +1360,8 @@ int commander_thread_main(int argc, char *argv[])
memset(&home, 0, sizeof(home));
if (stat_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed.\n");
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
}
@ -1445,6 +1451,7 @@ int commander_thread_main(int argc, char *argv[])
/* now initialized */
commander_initialized = true;
thread_running = true;
uint64_t start_time = hrt_absolute_time();
uint64_t failsave_ll_start_time = 0;
@ -1452,7 +1459,6 @@ int commander_thread_main(int argc, char *argv[])
bool state_changed = true;
bool param_init_forced = true;
while (!thread_should_exit) {
/* Get current values */

6
apps/drivers/drv_pwm_output.h
View File

@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
/** set debug level for servo IO */
#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4)
/** enable in-air restart */
#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5)
/** disable in-air restart */
#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

7
apps/drivers/ms5611/ms5611.cpp
View File

@ -144,6 +144,7 @@ private:
orb_advert_t _baro_topic;
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
@ -274,6 +275,7 @@ MS5611::MS5611(int bus) :
_msl_pressure(101325),
_baro_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
{
@ -647,6 +649,8 @@ MS5611::measure()
{
int ret;
perf_begin(_measure_perf);
/*
* In phase zero, request temperature; in other phases, request pressure.
*/
@ -664,6 +668,8 @@ MS5611::measure()
if (OK != ret)
perf_count(_comms_errors);
perf_end(_measure_perf);
return ret;
}
@ -689,6 +695,7 @@ MS5611::collect()
ret = transfer(&cmd, 1, &data[0], 3);
if (ret != OK) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}

59
apps/drivers/px4fmu/fmu.cpp
View File

@ -64,12 +64,14 @@
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
class PX4FMU : public device::CDev
{
@ -83,6 +85,7 @@ public:
~PX4FMU();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
virtual int init();
@ -338,6 +341,13 @@ PX4FMU::task_main()
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
log("starting");
/* loop until killed */
@ -363,8 +373,9 @@ PX4FMU::task_main()
_current_update_rate = _update_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
int ret = ::poll(&fds[0], 2, 10);
/* this would be bad... */
if (ret < 0) {
@ -429,6 +440,26 @@ PX4FMU::task_main()
/* update PWM servo armed status if armed and not locked down */
up_pwm_servo_arm(aa.armed && !aa.lockdown);
}
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
}
::close(_t_actuators);
@ -621,6 +652,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/*
this implements PWM output via a write() method, for compatibility
with px4io
*/
ssize_t
PX4FMU::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
uint16_t values[4];
if (count > 4) {
// we only have 4 PWM outputs on the FMU
count = 4;
}
// allow for misaligned values
memcpy(values, buffer, count*2);
for (uint8_t i=0; i<count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
}
void
PX4FMU::gpio_reset(void)
{

18
apps/drivers/px4io/px4io.cpp
View File

@ -367,7 +367,12 @@ PX4IO::init()
if (ret != OK)
return ret;
if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) {
/*
* in-air restart is only tried if the IO board reports it is
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
/* WARNING: COMMANDER app/vehicle status must be initialized.
* If this fails (or the app is not started), worst-case IO
@ -450,6 +455,7 @@ PX4IO::init()
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_ARM_OK |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
@ -1164,6 +1170,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
break;
case PWM_SERVO_INAIR_RESTART_ENABLE:
/* set the 'in-air restart' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
break;
case PWM_SERVO_INAIR_RESTART_DISABLE:
/* unset the 'in-air restart' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested rate */
if ((arg >= 50) && (arg <= 400)) {

4
apps/nshlib/Kconfig
View File

@ -55,6 +55,10 @@ config NSH_DISABLE_CP
bool "Disable cp"
default n
config NSH_DISABLE_CMP
bool "Disable cmp"
default n
config NSH_DISABLE_DD
bool "Disable dd"
default n

3
apps/nshlib/nsh.h
View File

@ -603,6 +603,9 @@ void nsh_usbtrace(void);
# ifndef CONFIG_NSH_DISABLE_CP
int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif
# ifndef CONFIG_NSH_DISABLE_CMP
int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif
# ifndef CONFIG_NSH_DISABLE_DD
int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif

81
apps/nshlib/nsh_fscmds.c
View File

@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
}
#endif
#endif
#if CONFIG_NFILE_DESCRIPTORS > 0
#ifndef CONFIG_NSH_DISABLE_CMP
int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
{
char *path1 = NULL;
char *path2 = NULL;
int fd1 = -1, fd2 = -1;
int ret = ERROR;
unsigned total_read = 0;
/* Get the full path to the two files */
path1 = nsh_getfullpath(vtbl, argv[1]);
if (!path1)
{
goto errout;
}
path2 = nsh_getfullpath(vtbl, argv[2]);
if (!path2)
{
goto errout;
}
/* Open the files for reading */
fd1 = open(path1, O_RDONLY);
if (fd1 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO);
goto errout;
}
fd2 = open(path2, O_RDONLY);
if (fd2 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO);
goto errout;
}
for (;;)
{
char buf1[128];
char buf2[128];
int nbytesread1 = read(fd1, buf1, sizeof(buf1));
int nbytesread2 = read(fd2, buf2, sizeof(buf2));
if (nbytesread1 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO);
goto errout;
}
if (nbytesread2 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO);
goto errout;
}
total_read += nbytesread1>nbytesread2?nbytesread2:nbytesread1;
if (nbytesread1 != nbytesread2 || memcmp(buf1, buf2, nbytesread1) != 0)
{
nsh_output(vtbl, "files differ: byte %u\n", total_read);
goto errout;
}
if (nbytesread1 < sizeof(buf1)) break;
}
ret = OK;
errout:
if (fd1 != -1) close(fd1);
if (fd2 != -1) close(fd2);
return ret;
}
#endif
#endif

3
apps/nshlib/nsh_parse.c
View File

@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] =
# ifndef CONFIG_NSH_DISABLE_CP
{ "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" },
# endif
# ifndef CONFIG_NSH_DISABLE_CMP
{ "cmp", cmd_cmp, 3, 3, "<path1> <path2>" },
# endif
#endif
#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE)

10
apps/px4io/mixer.cpp
View File

@ -301,7 +301,7 @@ mixer_handle_text(const void *buffer, size_t length)
px4io_mixdata *msg = (px4io_mixdata *)buffer;
debug("mixer text %u", length);
isr_debug(2, "mixer text %u", length);
if (length < sizeof(px4io_mixdata))
return;
@ -310,14 +310,14 @@ mixer_handle_text(const void *buffer, size_t length)
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
debug("reset");
isr_debug(2, "reset");
mixer_group.reset();
mixer_text_length = 0;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
@ -330,7 +330,7 @@ mixer_handle_text(const void *buffer, size_t length)
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
debug("buflen %u", mixer_text_length);
isr_debug(2, "buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
@ -342,7 +342,7 @@ mixer_handle_text(const void *buffer, size_t length)
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
debug("used %u", mixer_text_length - resid);
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)

1
apps/px4io/protocol.h
View File

@ -142,6 +142,7 @@
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */

24
apps/px4io/px4io.c
View File

@ -214,28 +214,34 @@ int user_start(int argc, char *argv[])
debug("Failed to setup SIGUSR1 handler\n");
}
/* run the mixer at ~300Hz (for now...) */
/* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
uint16_t counter=0;
/*
run the mixer at ~50Hz, using signals to run it early if
need be
*/
uint64_t last_debug_time = 0;
for (;;) {
/*
if we are not scheduled for 10ms then reset the I2C bus
if we are not scheduled for 30ms then reset the I2C bus
*/
hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL);
hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL);
// we use usleep() instead of poll() as poll() is not
// interrupted by signals in nuttx, whereas usleep() is
usleep(20000);
poll(NULL, 0, 3);
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
show_debug_messages();
if (counter++ == 800) {
counter = 0;
isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u",
if (hrt_absolute_time() - last_debug_time > 1000000) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u",
(unsigned)debug_level,
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)i2c_loop_resets);
last_debug_time = hrt_absolute_time();
}
}
}

3
apps/px4io/registers.c
View File

@ -142,7 +142,8 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)

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

@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib
CONFIGURED_APPS += systemlib/mixer
# Math library
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += mathlib
CONFIGURED_APPS += mathlib/CMSIS
CONFIGURED_APPS += examples/math_demo
endif
# Control library
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += controllib
CONFIGURED_APPS += examples/control_demo
CONFIGURED_APPS += examples/kalman_demo
endif
# System utility commands
CONFIGURED_APPS += systemcmds/reboot
@ -83,6 +87,8 @@ CONFIGURED_APPS += mavlink_onboard
CONFIGURED_APPS += commander
CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
@ -92,6 +98,7 @@ CONFIGURED_APPS += fixedwing_pos_control
CONFIGURED_APPS += position_estimator
CONFIGURED_APPS += attitude_estimator_ekf
CONFIGURED_APPS += hott_telemetry
endif
# Hacking tools
#CONFIGURED_APPS += system/i2c

3
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -369,7 +369,7 @@ CONFIG_I2C_WRITEREAD=y
# I2C configuration
#
CONFIG_I2C=y
CONFIG_I2C_POLLED=y
CONFIG_I2C_POLLED=n
CONFIG_I2C_TRANSFER=y
CONFIG_I2C_TRACE=n
CONFIG_I2C_RESET=y
@ -779,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32
CONFIG_NXFFS_TAILTHRESHOLD=2048
CONFIG_NXFFS_PREALLOCATED=y
CONFIG_FS_ROMFS=y
CONFIG_FS_BINFS=y
#
# SPI-based MMC/SD driver