mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 23:13:35 +08:00
Merge remote-tracking branch 'upstream/px4io-i2c' into new_state_machine
This commit is contained in:
+19
-13
@@ -429,7 +429,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||||||
// mavlink_log_info(mavlink_fd, buf);
|
// 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);
|
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||||
|
|
||||||
x[calibration_counter] = mag.x;
|
x[calibration_counter] = mag.x;
|
||||||
@@ -461,9 +463,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||||||
|
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
|
||||||
} else {
|
} else if (poll_ret == 0) {
|
||||||
/* any poll failure for 1s is a reason to abort */
|
/* 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;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -553,7 +555,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||||||
/* third beep by cal end routine */
|
/* third beep by cal end routine */
|
||||||
|
|
||||||
} else {
|
} 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 */
|
/* disable calibration mode */
|
||||||
@@ -598,14 +600,16 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||||||
/* wait blocking for new data */
|
/* wait blocking for new data */
|
||||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
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);
|
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||||
gyro_offset[0] += raw.gyro_rad_s[0];
|
gyro_offset[0] += raw.gyro_rad_s[0];
|
||||||
gyro_offset[1] += raw.gyro_rad_s[1];
|
gyro_offset[1] += raw.gyro_rad_s[1];
|
||||||
gyro_offset[2] += raw.gyro_rad_s[2];
|
gyro_offset[2] += raw.gyro_rad_s[2];
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
|
||||||
} else {
|
} else if (poll_ret == 0) {
|
||||||
/* any poll failure for 1s is a reason to abort */
|
/* any poll failure for 1s is a reason to abort */
|
||||||
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
||||||
return;
|
return;
|
||||||
@@ -706,14 +710,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||||||
/* wait blocking for new data */
|
/* wait blocking for new data */
|
||||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
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);
|
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||||
accel_offset[0] += raw.accelerometer_m_s2[0];
|
accel_offset[0] += raw.accelerometer_m_s2[0];
|
||||||
accel_offset[1] += raw.accelerometer_m_s2[1];
|
accel_offset[1] += raw.accelerometer_m_s2[1];
|
||||||
accel_offset[2] += raw.accelerometer_m_s2[2];
|
accel_offset[2] += raw.accelerometer_m_s2[2];
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
|
||||||
} else {
|
} else if (poll_ret == 0) {
|
||||||
/* any poll failure for 1s is a reason to abort */
|
/* any poll failure for 1s is a reason to abort */
|
||||||
mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
|
mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
|
||||||
return;
|
return;
|
||||||
@@ -1253,11 +1259,10 @@ int commander_main(int argc, char *argv[])
|
|||||||
thread_should_exit = false;
|
thread_should_exit = false;
|
||||||
daemon_task = task_spawn("commander",
|
daemon_task = task_spawn("commander",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_MAX - 50,
|
SCHED_PRIORITY_MAX - 40,
|
||||||
4000,
|
3000,
|
||||||
commander_thread_main,
|
commander_thread_main,
|
||||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
thread_running = true;
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1355,7 +1360,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
memset(&home, 0, sizeof(home));
|
memset(&home, 0, sizeof(home));
|
||||||
|
|
||||||
if (stat_pub < 0) {
|
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);
|
exit(ERROR);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1445,6 +1451,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
|
|
||||||
/* now initialized */
|
/* now initialized */
|
||||||
commander_initialized = true;
|
commander_initialized = true;
|
||||||
|
thread_running = true;
|
||||||
|
|
||||||
uint64_t start_time = hrt_absolute_time();
|
uint64_t start_time = hrt_absolute_time();
|
||||||
uint64_t failsave_ll_start_time = 0;
|
uint64_t failsave_ll_start_time = 0;
|
||||||
@@ -1452,7 +1459,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
bool state_changed = true;
|
bool state_changed = true;
|
||||||
bool param_init_forced = true;
|
bool param_init_forced = true;
|
||||||
|
|
||||||
|
|
||||||
while (!thread_should_exit) {
|
while (!thread_should_exit) {
|
||||||
|
|
||||||
/* Get current values */
|
/* Get current values */
|
||||||
|
|||||||
@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
|
|||||||
/** set debug level for servo IO */
|
/** set debug level for servo IO */
|
||||||
#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4)
|
#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 */
|
/** set a single servo to a specific value */
|
||||||
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
||||||
|
|
||||||
|
|||||||
@@ -144,6 +144,7 @@ private:
|
|||||||
orb_advert_t _baro_topic;
|
orb_advert_t _baro_topic;
|
||||||
|
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
|
perf_counter_t _measure_perf;
|
||||||
perf_counter_t _comms_errors;
|
perf_counter_t _comms_errors;
|
||||||
perf_counter_t _buffer_overflows;
|
perf_counter_t _buffer_overflows;
|
||||||
|
|
||||||
@@ -274,6 +275,7 @@ MS5611::MS5611(int bus) :
|
|||||||
_msl_pressure(101325),
|
_msl_pressure(101325),
|
||||||
_baro_topic(-1),
|
_baro_topic(-1),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
_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")),
|
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
|
||||||
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
|
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
|
||||||
{
|
{
|
||||||
@@ -647,6 +649,8 @@ MS5611::measure()
|
|||||||
{
|
{
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
|
perf_begin(_measure_perf);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* In phase zero, request temperature; in other phases, request pressure.
|
* In phase zero, request temperature; in other phases, request pressure.
|
||||||
*/
|
*/
|
||||||
@@ -664,6 +668,8 @@ MS5611::measure()
|
|||||||
if (OK != ret)
|
if (OK != ret)
|
||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
|
|
||||||
|
perf_end(_measure_perf);
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -689,6 +695,7 @@ MS5611::collect()
|
|||||||
ret = transfer(&cmd, 1, &data[0], 3);
|
ret = transfer(&cmd, 1, &data[0], 3);
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
|
perf_end(_sample_perf);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -64,12 +64,14 @@
|
|||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
#include <systemlib/mixer/mixer.h>
|
#include <systemlib/mixer/mixer.h>
|
||||||
#include <drivers/drv_mixer.h>
|
#include <drivers/drv_mixer.h>
|
||||||
|
#include <drivers/drv_rc_input.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
#include <uORB/topics/actuator_controls_effective.h>
|
#include <uORB/topics/actuator_controls_effective.h>
|
||||||
#include <uORB/topics/actuator_outputs.h>
|
#include <uORB/topics/actuator_outputs.h>
|
||||||
|
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
#include <systemlib/ppm_decode.h>
|
||||||
|
|
||||||
class PX4FMU : public device::CDev
|
class PX4FMU : public device::CDev
|
||||||
{
|
{
|
||||||
@@ -83,6 +85,7 @@ public:
|
|||||||
~PX4FMU();
|
~PX4FMU();
|
||||||
|
|
||||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
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();
|
virtual int init();
|
||||||
|
|
||||||
@@ -338,6 +341,13 @@ PX4FMU::task_main()
|
|||||||
|
|
||||||
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
|
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");
|
log("starting");
|
||||||
|
|
||||||
/* loop until killed */
|
/* loop until killed */
|
||||||
@@ -363,8 +373,9 @@ PX4FMU::task_main()
|
|||||||
_current_update_rate = _update_rate;
|
_current_update_rate = _update_rate;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* sleep waiting for data, but no more than a second */
|
/* sleep waiting for data, stopping to check for PPM
|
||||||
int ret = ::poll(&fds[0], 2, 1000);
|
* input at 100Hz */
|
||||||
|
int ret = ::poll(&fds[0], 2, 10);
|
||||||
|
|
||||||
/* this would be bad... */
|
/* this would be bad... */
|
||||||
if (ret < 0) {
|
if (ret < 0) {
|
||||||
@@ -429,6 +440,26 @@ PX4FMU::task_main()
|
|||||||
/* update PWM servo armed status if armed and not locked down */
|
/* update PWM servo armed status if armed and not locked down */
|
||||||
up_pwm_servo_arm(aa.armed && !aa.lockdown);
|
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);
|
::close(_t_actuators);
|
||||||
@@ -621,6 +652,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||||||
return ret;
|
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
|
void
|
||||||
PX4FMU::gpio_reset(void)
|
PX4FMU::gpio_reset(void)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -367,7 +367,12 @@ PX4IO::init()
|
|||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
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.
|
/* WARNING: COMMANDER app/vehicle status must be initialized.
|
||||||
* If this fails (or the app is not started), worst-case IO
|
* If this fails (or the app is not started), worst-case IO
|
||||||
@@ -450,6 +455,7 @@ PX4IO::init()
|
|||||||
/* dis-arm IO before touching anything */
|
/* dis-arm IO before touching anything */
|
||||||
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
||||||
PX4IO_P_SETUP_ARMING_ARM_OK |
|
PX4IO_P_SETUP_ARMING_ARM_OK |
|
||||||
|
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
|
||||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
||||||
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
|
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);
|
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
|
||||||
break;
|
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:
|
case PWM_SERVO_SET_UPDATE_RATE:
|
||||||
/* set the requested rate */
|
/* set the requested rate */
|
||||||
if ((arg >= 50) && (arg <= 400)) {
|
if ((arg >= 50) && (arg <= 400)) {
|
||||||
|
|||||||
@@ -55,6 +55,10 @@ config NSH_DISABLE_CP
|
|||||||
bool "Disable cp"
|
bool "Disable cp"
|
||||||
default n
|
default n
|
||||||
|
|
||||||
|
config NSH_DISABLE_CMP
|
||||||
|
bool "Disable cmp"
|
||||||
|
default n
|
||||||
|
|
||||||
config NSH_DISABLE_DD
|
config NSH_DISABLE_DD
|
||||||
bool "Disable dd"
|
bool "Disable dd"
|
||||||
default n
|
default n
|
||||||
|
|||||||
@@ -603,6 +603,9 @@ void nsh_usbtrace(void);
|
|||||||
# ifndef CONFIG_NSH_DISABLE_CP
|
# ifndef CONFIG_NSH_DISABLE_CP
|
||||||
int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
|
int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
|
||||||
# endif
|
# 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
|
# ifndef CONFIG_NSH_DISABLE_DD
|
||||||
int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
|
int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
|
||||||
# endif
|
# endif
|
||||||
|
|||||||
@@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
#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
|
||||||
|
|||||||
@@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] =
|
|||||||
# ifndef CONFIG_NSH_DISABLE_CP
|
# ifndef CONFIG_NSH_DISABLE_CP
|
||||||
{ "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" },
|
{ "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" },
|
||||||
# endif
|
# endif
|
||||||
|
# ifndef CONFIG_NSH_DISABLE_CMP
|
||||||
|
{ "cmp", cmd_cmp, 3, 3, "<path1> <path2>" },
|
||||||
|
# endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE)
|
#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE)
|
||||||
|
|||||||
@@ -301,7 +301,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||||||
|
|
||||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||||
|
|
||||||
debug("mixer text %u", length);
|
isr_debug(2, "mixer text %u", length);
|
||||||
|
|
||||||
if (length < sizeof(px4io_mixdata))
|
if (length < sizeof(px4io_mixdata))
|
||||||
return;
|
return;
|
||||||
@@ -310,14 +310,14 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||||||
|
|
||||||
switch (msg->action) {
|
switch (msg->action) {
|
||||||
case F2I_MIXER_ACTION_RESET:
|
case F2I_MIXER_ACTION_RESET:
|
||||||
debug("reset");
|
isr_debug(2, "reset");
|
||||||
mixer_group.reset();
|
mixer_group.reset();
|
||||||
mixer_text_length = 0;
|
mixer_text_length = 0;
|
||||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||||
|
|
||||||
/* FALLTHROUGH */
|
/* FALLTHROUGH */
|
||||||
case F2I_MIXER_ACTION_APPEND:
|
case F2I_MIXER_ACTION_APPEND:
|
||||||
debug("append %d", length);
|
isr_debug(2, "append %d", length);
|
||||||
|
|
||||||
/* check for overflow - this is really fatal */
|
/* check for overflow - this is really fatal */
|
||||||
/* XXX could add just what will fit & try to parse, then repeat... */
|
/* 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);
|
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';
|
||||||
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 */
|
/* process the text buffer, adding new mixers as their descriptions can be parsed */
|
||||||
unsigned resid = mixer_text_length;
|
unsigned resid = mixer_text_length;
|
||||||
@@ -342,7 +342,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||||||
/* ideally, this should test resid == 0 ? */
|
/* ideally, this should test resid == 0 ? */
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
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 */
|
/* copy any leftover text to the base of the buffer for re-use */
|
||||||
if (resid > 0)
|
if (resid > 0)
|
||||||
|
|||||||
@@ -142,6 +142,7 @@
|
|||||||
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
|
#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_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_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_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||||
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
|
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
|
||||||
|
|||||||
+15
-9
@@ -214,28 +214,34 @@ int user_start(int argc, char *argv[])
|
|||||||
debug("Failed to setup SIGUSR1 handler\n");
|
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 */
|
run the mixer at ~50Hz, using signals to run it early if
|
||||||
uint16_t counter=0;
|
need be
|
||||||
|
*/
|
||||||
|
uint64_t last_debug_time = 0;
|
||||||
for (;;) {
|
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);
|
perf_begin(mixer_perf);
|
||||||
mixer_tick();
|
mixer_tick();
|
||||||
perf_end(mixer_perf);
|
perf_end(mixer_perf);
|
||||||
|
|
||||||
show_debug_messages();
|
show_debug_messages();
|
||||||
if (counter++ == 800) {
|
if (hrt_absolute_time() - last_debug_time > 1000000) {
|
||||||
counter = 0;
|
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u",
|
||||||
isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u",
|
|
||||||
(unsigned)debug_level,
|
(unsigned)debug_level,
|
||||||
(unsigned)r_status_flags,
|
(unsigned)r_status_flags,
|
||||||
(unsigned)r_setup_arming,
|
(unsigned)r_setup_arming,
|
||||||
(unsigned)r_setup_features,
|
(unsigned)r_setup_features,
|
||||||
(unsigned)i2c_loop_resets);
|
(unsigned)i2c_loop_resets);
|
||||||
|
last_debug_time = hrt_absolute_time();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -142,7 +142,8 @@ volatile uint16_t r_page_setup[] =
|
|||||||
|
|
||||||
#define PX4IO_P_SETUP_FEATURES_VALID (0)
|
#define PX4IO_P_SETUP_FEATURES_VALID (0)
|
||||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
|
#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_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
|
||||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||||
|
|
||||||
|
|||||||
@@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib
|
|||||||
CONFIGURED_APPS += systemlib/mixer
|
CONFIGURED_APPS += systemlib/mixer
|
||||||
|
|
||||||
# Math library
|
# Math library
|
||||||
|
ifneq ($(CONFIG_APM),y)
|
||||||
CONFIGURED_APPS += mathlib
|
CONFIGURED_APPS += mathlib
|
||||||
CONFIGURED_APPS += mathlib/CMSIS
|
CONFIGURED_APPS += mathlib/CMSIS
|
||||||
CONFIGURED_APPS += examples/math_demo
|
CONFIGURED_APPS += examples/math_demo
|
||||||
|
endif
|
||||||
|
|
||||||
# Control library
|
# Control library
|
||||||
|
ifneq ($(CONFIG_APM),y)
|
||||||
CONFIGURED_APPS += controllib
|
CONFIGURED_APPS += controllib
|
||||||
CONFIGURED_APPS += examples/control_demo
|
CONFIGURED_APPS += examples/control_demo
|
||||||
CONFIGURED_APPS += examples/kalman_demo
|
CONFIGURED_APPS += examples/kalman_demo
|
||||||
|
endif
|
||||||
|
|
||||||
# System utility commands
|
# System utility commands
|
||||||
CONFIGURED_APPS += systemcmds/reboot
|
CONFIGURED_APPS += systemcmds/reboot
|
||||||
@@ -83,6 +87,8 @@ CONFIGURED_APPS += mavlink_onboard
|
|||||||
CONFIGURED_APPS += commander
|
CONFIGURED_APPS += commander
|
||||||
CONFIGURED_APPS += sdlog
|
CONFIGURED_APPS += sdlog
|
||||||
CONFIGURED_APPS += sensors
|
CONFIGURED_APPS += sensors
|
||||||
|
|
||||||
|
ifneq ($(CONFIG_APM),y)
|
||||||
CONFIGURED_APPS += ardrone_interface
|
CONFIGURED_APPS += ardrone_interface
|
||||||
CONFIGURED_APPS += multirotor_att_control
|
CONFIGURED_APPS += multirotor_att_control
|
||||||
CONFIGURED_APPS += multirotor_pos_control
|
CONFIGURED_APPS += multirotor_pos_control
|
||||||
@@ -92,6 +98,7 @@ CONFIGURED_APPS += fixedwing_pos_control
|
|||||||
CONFIGURED_APPS += position_estimator
|
CONFIGURED_APPS += position_estimator
|
||||||
CONFIGURED_APPS += attitude_estimator_ekf
|
CONFIGURED_APPS += attitude_estimator_ekf
|
||||||
CONFIGURED_APPS += hott_telemetry
|
CONFIGURED_APPS += hott_telemetry
|
||||||
|
endif
|
||||||
|
|
||||||
# Hacking tools
|
# Hacking tools
|
||||||
#CONFIGURED_APPS += system/i2c
|
#CONFIGURED_APPS += system/i2c
|
||||||
|
|||||||
@@ -369,7 +369,7 @@ CONFIG_I2C_WRITEREAD=y
|
|||||||
# I2C configuration
|
# I2C configuration
|
||||||
#
|
#
|
||||||
CONFIG_I2C=y
|
CONFIG_I2C=y
|
||||||
CONFIG_I2C_POLLED=y
|
CONFIG_I2C_POLLED=n
|
||||||
CONFIG_I2C_TRANSFER=y
|
CONFIG_I2C_TRANSFER=y
|
||||||
CONFIG_I2C_TRACE=n
|
CONFIG_I2C_TRACE=n
|
||||||
CONFIG_I2C_RESET=y
|
CONFIG_I2C_RESET=y
|
||||||
@@ -779,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32
|
|||||||
CONFIG_NXFFS_TAILTHRESHOLD=2048
|
CONFIG_NXFFS_TAILTHRESHOLD=2048
|
||||||
CONFIG_NXFFS_PREALLOCATED=y
|
CONFIG_NXFFS_PREALLOCATED=y
|
||||||
CONFIG_FS_ROMFS=y
|
CONFIG_FS_ROMFS=y
|
||||||
|
CONFIG_FS_BINFS=y
|
||||||
|
|
||||||
#
|
#
|
||||||
# SPI-based MMC/SD driver
|
# SPI-based MMC/SD driver
|
||||||
|
|||||||
Reference in New Issue
Block a user