forked from Archive/PX4-Autopilot
Merge branch 'cleanup' into beta
This commit is contained in:
commit
a81cf70460
|
@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
|
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
|
||||||
{
|
{
|
||||||
// enable debug() calls
|
// enable debug() calls
|
||||||
_debug_enabled = true;
|
_debug_enabled = false;
|
||||||
|
|
||||||
// work_cancel in the dtor will explode if we don't do this...
|
// work_cancel in the dtor will explode if we don't do this...
|
||||||
memset(&_work, 0, sizeof(_work));
|
memset(&_work, 0, sizeof(_work));
|
||||||
|
|
|
@ -451,7 +451,7 @@ private:
|
||||||
namespace
|
namespace
|
||||||
{
|
{
|
||||||
|
|
||||||
PX4IO *g_dev;
|
PX4IO *g_dev = nullptr;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||||
/* open MAVLink text channel */
|
/* open MAVLink text channel */
|
||||||
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||||
|
|
||||||
_debug_enabled = true;
|
_debug_enabled = false;
|
||||||
_servorail_status.rssi_v = 0;
|
_servorail_status.rssi_v = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -580,6 +580,12 @@ PX4IO::init()
|
||||||
/* get some parameters */
|
/* get some parameters */
|
||||||
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
|
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
|
||||||
|
|
||||||
|
if (protocol == _io_reg_get_error) {
|
||||||
|
log("failed to communicate with IO");
|
||||||
|
mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
if (protocol != PX4IO_PROTOCOL_VERSION) {
|
if (protocol != PX4IO_PROTOCOL_VERSION) {
|
||||||
log("protocol/firmware mismatch");
|
log("protocol/firmware mismatch");
|
||||||
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
|
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
|
||||||
|
@ -774,8 +780,6 @@ PX4IO::task_main()
|
||||||
hrt_abstime poll_last = 0;
|
hrt_abstime poll_last = 0;
|
||||||
hrt_abstime orb_check_last = 0;
|
hrt_abstime orb_check_last = 0;
|
||||||
|
|
||||||
log("starting");
|
|
||||||
|
|
||||||
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -809,8 +813,6 @@ PX4IO::task_main()
|
||||||
fds[0].fd = _t_actuator_controls_0;
|
fds[0].fd = _t_actuator_controls_0;
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
|
|
||||||
log("ready");
|
|
||||||
|
|
||||||
/* lock against the ioctl handler */
|
/* lock against the ioctl handler */
|
||||||
lock();
|
lock();
|
||||||
|
|
||||||
|
@ -1673,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
|
||||||
total_len++;
|
total_len++;
|
||||||
}
|
}
|
||||||
|
|
||||||
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
|
int ret;
|
||||||
|
|
||||||
|
for (int i = 0; i < 30; i++) {
|
||||||
|
/* failed, but give it a 2nd shot */
|
||||||
|
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
|
||||||
|
|
||||||
|
if (ret) {
|
||||||
|
usleep(333);
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* print mixer chunk */
|
/* print mixer chunk */
|
||||||
if (debuglevel > 5 || ret) {
|
if (debuglevel > 5 || ret) {
|
||||||
|
@ -1697,7 +1710,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
|
||||||
msg->text[0] = '\n';
|
msg->text[0] = '\n';
|
||||||
msg->text[1] = '\0';
|
msg->text[1] = '\0';
|
||||||
|
|
||||||
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
|
int ret;
|
||||||
|
|
||||||
|
for (int i = 0; i < 30; i++) {
|
||||||
|
/* failed, but give it a 2nd shot */
|
||||||
|
ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
|
||||||
|
|
||||||
|
if (ret) {
|
||||||
|
usleep(333);
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (ret)
|
if (ret)
|
||||||
return ret;
|
return ret;
|
||||||
|
@ -2699,6 +2723,7 @@ px4io_main(int argc, char *argv[])
|
||||||
printf("[px4io] loaded, detaching first\n");
|
printf("[px4io] loaded, detaching first\n");
|
||||||
/* stop the driver */
|
/* stop the driver */
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
PX4IO_Uploader *up;
|
PX4IO_Uploader *up;
|
||||||
|
@ -2782,10 +2807,6 @@ px4io_main(int argc, char *argv[])
|
||||||
delete interface;
|
delete interface;
|
||||||
errx(1, "driver alloc failed");
|
errx(1, "driver alloc failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (OK != g_dev->init()) {
|
|
||||||
warnx("driver init failed, still trying..");
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t arg = atol(argv[2]);
|
uint16_t arg = atol(argv[2]);
|
||||||
|
@ -2797,6 +2818,7 @@ px4io_main(int argc, char *argv[])
|
||||||
|
|
||||||
// tear down the px4io instance
|
// tear down the px4io instance
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
|
g_dev = nullptr;
|
||||||
|
|
||||||
// upload the specified firmware
|
// upload the specified firmware
|
||||||
const char *fn[2];
|
const char *fn[2];
|
||||||
|
@ -2855,6 +2877,7 @@ px4io_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* stop the driver */
|
/* stop the driver */
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
|
g_dev = nullptr;
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -121,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
cfsetspeed(&t, 115200);
|
cfsetspeed(&t, 115200);
|
||||||
tcsetattr(_io_fd, TCSANOW, &t);
|
tcsetattr(_io_fd, TCSANOW, &t);
|
||||||
|
|
||||||
/* look for the bootloader */
|
/* look for the bootloader for 150 ms */
|
||||||
|
for (int i = 0; i < 15; i++) {
|
||||||
ret = sync();
|
ret = sync();
|
||||||
|
if (ret == OK) {
|
||||||
|
break;
|
||||||
|
} else {
|
||||||
|
usleep(10000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
/* this is immediately fatal */
|
/* this is immediately fatal */
|
||||||
|
|
|
@ -91,7 +91,7 @@ private:
|
||||||
void drain();
|
void drain();
|
||||||
int send(uint8_t c);
|
int send(uint8_t c);
|
||||||
int send(uint8_t *p, unsigned count);
|
int send(uint8_t *p, unsigned count);
|
||||||
int get_sync(unsigned timeout = 1000);
|
int get_sync(unsigned timeout = 40);
|
||||||
int sync();
|
int sync();
|
||||||
int get_info(int param, uint32_t &val);
|
int get_info(int param, uint32_t &val);
|
||||||
int erase();
|
int erase();
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -71,6 +71,7 @@ extern "C" {
|
||||||
static bool mixer_servos_armed = false;
|
static bool mixer_servos_armed = false;
|
||||||
static bool should_arm = false;
|
static bool should_arm = false;
|
||||||
static bool should_always_enable_pwm = false;
|
static bool should_always_enable_pwm = false;
|
||||||
|
static volatile bool in_mixer = false;
|
||||||
|
|
||||||
/* selected control values and count for mixing */
|
/* selected control values and count for mixing */
|
||||||
enum mixer_source {
|
enum mixer_source {
|
||||||
|
@ -95,6 +96,7 @@ static void mixer_set_failsafe();
|
||||||
void
|
void
|
||||||
mixer_tick(void)
|
mixer_tick(void)
|
||||||
{
|
{
|
||||||
|
|
||||||
/* check that we are receiving fresh data from the FMU */
|
/* check that we are receiving fresh data from the FMU */
|
||||||
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
|
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
|
||||||
|
|
||||||
|
@ -199,13 +201,17 @@ mixer_tick(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
} else if (source != MIX_NONE) {
|
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||||
|
|
||||||
float outputs[PX4IO_SERVO_COUNT];
|
float outputs[PX4IO_SERVO_COUNT];
|
||||||
unsigned mixed;
|
unsigned mixed;
|
||||||
|
|
||||||
/* mix */
|
/* mix */
|
||||||
|
|
||||||
|
/* poor mans mutex */
|
||||||
|
in_mixer = true;
|
||||||
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||||
|
in_mixer = false;
|
||||||
|
|
||||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||||
|
|
||||||
|
@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle,
|
||||||
static char mixer_text[256]; /* large enough for one mixer */
|
static char mixer_text[256]; /* large enough for one mixer */
|
||||||
static unsigned mixer_text_length = 0;
|
static unsigned mixer_text_length = 0;
|
||||||
|
|
||||||
void
|
int
|
||||||
mixer_handle_text(const void *buffer, size_t length)
|
mixer_handle_text(const void *buffer, size_t length)
|
||||||
{
|
{
|
||||||
/* do not allow a mixer change while safety off */
|
/* do not allow a mixer change while safety off */
|
||||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
|
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
|
||||||
return;
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* abort if we're in the mixer */
|
||||||
|
if (in_mixer) {
|
||||||
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||||
|
@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
||||||
isr_debug(2, "mix txt %u", length);
|
isr_debug(2, "mix txt %u", length);
|
||||||
|
|
||||||
if (length < sizeof(px4io_mixdata))
|
if (length < sizeof(px4io_mixdata))
|
||||||
return;
|
return 0;
|
||||||
|
|
||||||
unsigned text_length = length - sizeof(px4io_mixdata);
|
unsigned text_length = length - sizeof(px4io_mixdata);
|
||||||
|
|
||||||
|
@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length)
|
||||||
case F2I_MIXER_ACTION_APPEND:
|
case F2I_MIXER_ACTION_APPEND:
|
||||||
isr_debug(2, "append %d", length);
|
isr_debug(2, "append %d", length);
|
||||||
|
|
||||||
|
/* disable mixing during the update */
|
||||||
|
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||||
|
|
||||||
/* check for overflow - this would be really fatal */
|
/* check for overflow - this would be really fatal */
|
||||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
|
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
|
||||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||||
return;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* append mixer text and nul-terminate */
|
/* append mixer text and nul-terminate, guard against overflow */
|
||||||
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
|
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
|
||||||
mixer_text_length += text_length;
|
mixer_text_length += text_length;
|
||||||
mixer_text[mixer_text_length] = '\0';
|
mixer_text[mixer_text_length] = '\0';
|
||||||
|
@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length)
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
static void
|
||||||
|
|
|
@ -295,10 +295,12 @@ user_start(int argc, char *argv[])
|
||||||
|
|
||||||
check_reboot();
|
check_reboot();
|
||||||
|
|
||||||
/* check for debug activity */
|
/* check for debug activity (default: none) */
|
||||||
show_debug_messages();
|
show_debug_messages();
|
||||||
|
|
||||||
/* post debug state at ~1Hz */
|
/* post debug state at ~1Hz - this is via an auxiliary serial port
|
||||||
|
* DEFAULTS TO OFF!
|
||||||
|
*/
|
||||||
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
|
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
|
||||||
|
|
||||||
struct mallinfo minfo = mallinfo();
|
struct mallinfo minfo = mallinfo();
|
||||||
|
|
|
@ -178,7 +178,7 @@ extern pwm_limit_t pwm_limit;
|
||||||
* Mixer
|
* Mixer
|
||||||
*/
|
*/
|
||||||
extern void mixer_tick(void);
|
extern void mixer_tick(void);
|
||||||
extern void mixer_handle_text(const void *buffer, size_t length);
|
extern int mixer_handle_text(const void *buffer, size_t length);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Safety switch/LED.
|
* Safety switch/LED.
|
||||||
|
|
|
@ -380,7 +380,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
||||||
|
|
||||||
/* handle text going to the mixer parser */
|
/* handle text going to the mixer parser */
|
||||||
case PX4IO_PAGE_MIXERLOAD:
|
case PX4IO_PAGE_MIXERLOAD:
|
||||||
mixer_handle_text(values, num_values * sizeof(*values));
|
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
||||||
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
|
||||||
|
return mixer_handle_text(values, num_values * sizeof(*values));
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -507,8 +510,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||||
|
|
||||||
case PX4IO_P_SETUP_REBOOT_BL:
|
case PX4IO_P_SETUP_REBOOT_BL:
|
||||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
||||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
|
||||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
|
||||||
// don't allow reboot while armed
|
// don't allow reboot while armed
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -538,8 +540,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||||
* do not allow a RC config change while outputs armed
|
* do not allow a RC config change while outputs armed
|
||||||
*/
|
*/
|
||||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
|
||||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
|
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
|
||||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -795,7 +795,6 @@ Sensors::accel_init()
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
warnx("using system accel");
|
|
||||||
close(fd);
|
close(fd);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -835,7 +834,6 @@ Sensors::gyro_init()
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
warnx("using system gyro");
|
|
||||||
close(fd);
|
close(fd);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1505,9 +1503,6 @@ void
|
||||||
Sensors::task_main()
|
Sensors::task_main()
|
||||||
{
|
{
|
||||||
|
|
||||||
/* inform about start */
|
|
||||||
warnx("Initializing..");
|
|
||||||
|
|
||||||
/* start individual sensors */
|
/* start individual sensors */
|
||||||
accel_init();
|
accel_init();
|
||||||
gyro_init();
|
gyro_init();
|
||||||
|
|
Loading…
Reference in New Issue