forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/px4io-i2c' into px4io-i2c
This commit is contained in:
commit
e0345f7abe
|
@ -174,11 +174,15 @@ end
|
|||
define showtaskstack
|
||||
set $task = (struct _TCB *)$arg0
|
||||
|
||||
if $task == &g_idletcb
|
||||
printf "can't measure idle stack\n"
|
||||
else
|
||||
set $stack_free = 0
|
||||
while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
|
||||
set $stack_free = $stack_free + 1
|
||||
end
|
||||
printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
|
||||
end
|
||||
end
|
||||
|
||||
#
|
||||
|
|
|
@ -22,7 +22,7 @@ end
|
|||
|
||||
define _perf_print
|
||||
set $hdr = (struct perf_ctr_header *)$arg0
|
||||
printf "%p\n", $hdr
|
||||
#printf "%p\n", $hdr
|
||||
printf "%s: ", $hdr->name
|
||||
# PC_COUNT
|
||||
if $hdr->type == 0
|
||||
|
|
|
@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
|
|||
*/
|
||||
__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
|
||||
|
||||
/*
|
||||
* Compute the delta between a timestamp taken in the past
|
||||
* and now.
|
||||
*
|
||||
* This function is safe to use even if the timestamp is updated
|
||||
* by an interrupt during execution.
|
||||
*/
|
||||
__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
|
||||
|
||||
/*
|
||||
* Store the absolute time in an interrupt-safe fashion.
|
||||
*
|
||||
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
|
||||
*/
|
||||
__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
|
||||
|
||||
/*
|
||||
* Call callout(arg) after delay has elapsed.
|
||||
*
|
||||
|
|
|
@ -96,9 +96,12 @@ public:
|
|||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
virtual ssize_t write(file *filp, const char *buffer, size_t len);
|
||||
|
||||
void print_status();
|
||||
|
||||
private:
|
||||
// XXX
|
||||
unsigned _max_actuators;
|
||||
unsigned _max_controls;
|
||||
unsigned _max_rc_input;
|
||||
unsigned _max_relays;
|
||||
unsigned _max_transfer;
|
||||
|
@ -275,18 +278,20 @@ PX4IO *g_dev;
|
|||
PX4IO::PX4IO() :
|
||||
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
|
||||
_max_actuators(0),
|
||||
_max_controls(0),
|
||||
_max_rc_input(0),
|
||||
_max_relays(0),
|
||||
_max_transfer(16), /* sensible default */
|
||||
_update_interval(0),
|
||||
_status(0),
|
||||
_alarms(0),
|
||||
_task(-1),
|
||||
_task_should_exit(false),
|
||||
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
|
||||
_status(0),
|
||||
_alarms(0),
|
||||
_t_actuators(-1),
|
||||
_t_armed(-1),
|
||||
_t_vstatus(-1),
|
||||
_t_param(-1),
|
||||
_to_input_rc(0),
|
||||
_to_actuators_effective(0),
|
||||
_to_outputs(0),
|
||||
|
@ -339,6 +344,7 @@ PX4IO::init()
|
|||
|
||||
/* get some parameters */
|
||||
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
|
||||
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
|
||||
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
|
||||
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
|
||||
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
|
||||
|
@ -634,11 +640,11 @@ PX4IO::io_set_control_state()
|
|||
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||
ORB_ID(actuator_controls_1), _t_actuators, &controls);
|
||||
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
for (unsigned i = 0; i < _max_controls; i++)
|
||||
regs[i] = FLOAT_TO_REG(controls.control[i]);
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
|
||||
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -688,21 +694,26 @@ PX4IO::io_set_rc_config()
|
|||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
input_map[i] = -1;
|
||||
|
||||
/*
|
||||
* NOTE: The indices for mapped channels are 1-based
|
||||
* for compatibility reasons with existing
|
||||
* autopilots / GCS'.
|
||||
*/
|
||||
param_get(param_find("RC_MAP_ROLL"), &ichan);
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan] = 0;
|
||||
input_map[ichan - 1] = 0;
|
||||
|
||||
param_get(param_find("RC_MAP_PITCH"), &ichan);
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan] = 1;
|
||||
input_map[ichan - 1] = 1;
|
||||
|
||||
param_get(param_find("RC_MAP_YAW"), &ichan);
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan] = 2;
|
||||
input_map[ichan - 1] = 2;
|
||||
|
||||
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan] = 3;
|
||||
input_map[ichan - 1] = 3;
|
||||
|
||||
ichan = 4;
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
|
@ -812,6 +823,8 @@ PX4IO::io_handle_alarms(uint16_t alarms)
|
|||
|
||||
/* set new alarms state */
|
||||
_alarms = alarms;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -1141,18 +1154,131 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
|
|||
|
||||
} while (buflen > 0);
|
||||
|
||||
debug("mixer upload OK");
|
||||
|
||||
/* check for the mixer-OK flag */
|
||||
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)
|
||||
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
|
||||
debug("mixer upload OK");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
debug("mixer rejected by IO");
|
||||
}
|
||||
|
||||
/* load must have failed for some reason */
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::print_status()
|
||||
{
|
||||
/* basic configuration */
|
||||
printf("protocol %u software %u bootloader %u buffer %uB\n",
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
|
||||
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
|
||||
|
||||
/* status */
|
||||
printf("%u bytes free\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
|
||||
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
|
||||
flags,
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"));
|
||||
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
|
||||
printf("alarms 0x%04x%s%s%s%s%s%s\n",
|
||||
alarms,
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""));
|
||||
printf("vbatt %u ibatt %u\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
|
||||
printf("actuators");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
|
||||
printf("\n");
|
||||
printf("servos");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
|
||||
printf("\n");
|
||||
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
|
||||
printf("%d raw R/C inputs", raw_inputs);
|
||||
for (unsigned i = 0; i < raw_inputs; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
|
||||
printf("\n");
|
||||
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
|
||||
printf("mapped R/C inputs 0x%04x", mapped_inputs);
|
||||
for (unsigned i = 0; i < _max_rc_input; i++) {
|
||||
if (mapped_inputs & (1 << i))
|
||||
printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
|
||||
}
|
||||
printf("\n");
|
||||
uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
|
||||
printf("ADC inputs");
|
||||
for (unsigned i = 0; i < adc_inputs; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
|
||||
printf("\n");
|
||||
|
||||
/* setup and state */
|
||||
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
|
||||
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
|
||||
printf("arming 0x%04x%s%s%s%s\n",
|
||||
arming,
|
||||
((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
|
||||
printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n",
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
|
||||
printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
|
||||
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
|
||||
printf("controls");
|
||||
for (unsigned i = 0; i < _max_controls; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
|
||||
printf("\n");
|
||||
for (unsigned i = 0; i < _max_rc_input; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
|
||||
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
|
||||
i,
|
||||
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
|
||||
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
|
||||
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
|
||||
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
|
||||
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
|
||||
options,
|
||||
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
|
||||
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
|
||||
}
|
||||
printf("failsafe");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||
{
|
||||
|
@ -1294,7 +1420,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
|||
}
|
||||
|
||||
default:
|
||||
/* not a recognised value */
|
||||
/* not a recognized value */
|
||||
ret = -ENOTTY;
|
||||
}
|
||||
|
||||
|
@ -1458,10 +1584,12 @@ px4io_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
printf("[px4io] loaded\n");
|
||||
else
|
||||
g_dev->print_status();
|
||||
} else {
|
||||
printf("[px4io] not loaded\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
|
|
@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
|||
ts->tv_nsec = abstime * 1000;
|
||||
}
|
||||
|
||||
/*
|
||||
* Compare a time value with the current time.
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_elapsed_time(const volatile hrt_abstime *then)
|
||||
{
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
hrt_abstime delta = hrt_absolute_time() - *then;
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
return delta;
|
||||
}
|
||||
|
||||
/*
|
||||
* Store the absolute time in an interrupt-safe fashion
|
||||
*/
|
||||
hrt_abstime
|
||||
hrt_store_absolute_time(volatile hrt_abstime *now)
|
||||
{
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
hrt_abstime ts = hrt_absolute_time();
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
return ts;
|
||||
}
|
||||
|
||||
/*
|
||||
* Initalise the high-resolution timing module.
|
||||
*/
|
||||
|
|
|
@ -247,8 +247,8 @@ void KalmanNav::update()
|
|||
// output
|
||||
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
|
||||
_outTimeStamp = newTimeStamp;
|
||||
printf("nav: %4d Hz, miss #: %4d\n",
|
||||
_navFrames / 10, _miss / 10);
|
||||
//printf("nav: %4d Hz, miss #: %4d\n",
|
||||
// _navFrames / 10, _miss / 10);
|
||||
_navFrames = 0;
|
||||
_miss = 0;
|
||||
}
|
||||
|
|
|
@ -1,42 +0,0 @@
|
|||
/* List of application requirements, generated during make context. */
|
||||
{ "math_demo", SCHED_PRIORITY_DEFAULT, 8192, math_demo_main },
|
||||
{ "control_demo", SCHED_PRIORITY_DEFAULT, 2048, control_demo_main },
|
||||
{ "kalman_demo", SCHED_PRIORITY_MAX - 30, 2048, kalman_demo_main },
|
||||
{ "reboot", SCHED_PRIORITY_DEFAULT, 2048, reboot_main },
|
||||
{ "perf", SCHED_PRIORITY_DEFAULT, 2048, perf_main },
|
||||
{ "top", SCHED_PRIORITY_DEFAULT - 10, 3000, top_main },
|
||||
{ "boardinfo", SCHED_PRIORITY_DEFAULT, 2048, boardinfo_main },
|
||||
{ "mixer", SCHED_PRIORITY_DEFAULT, 4096, mixer_main },
|
||||
{ "eeprom", SCHED_PRIORITY_DEFAULT, 4096, eeprom_main },
|
||||
{ "param", SCHED_PRIORITY_DEFAULT, 4096, param_main },
|
||||
{ "bl_update", SCHED_PRIORITY_DEFAULT, 4096, bl_update_main },
|
||||
{ "preflight_check", SCHED_PRIORITY_DEFAULT, 2048, preflight_check_main },
|
||||
{ "delay_test", SCHED_PRIORITY_DEFAULT, 2048, delay_test_main },
|
||||
{ "uorb", SCHED_PRIORITY_DEFAULT, 4096, uorb_main },
|
||||
{ "mavlink", SCHED_PRIORITY_DEFAULT, 2048, mavlink_main },
|
||||
{ "mavlink_onboard", SCHED_PRIORITY_DEFAULT, 2048, mavlink_onboard_main },
|
||||
{ "gps", SCHED_PRIORITY_DEFAULT, 2048, gps_main },
|
||||
{ "commander", SCHED_PRIORITY_MAX - 30, 2048, commander_main },
|
||||
{ "sdlog", SCHED_PRIORITY_MAX - 30, 2048, sdlog_main },
|
||||
{ "sensors", SCHED_PRIORITY_MAX-5, 4096, sensors_main },
|
||||
{ "ardrone_interface", SCHED_PRIORITY_MAX - 15, 2048, ardrone_interface_main },
|
||||
{ "multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, multirotor_att_control_main },
|
||||
{ "multirotor_pos_control", SCHED_PRIORITY_MAX - 25, 2048, multirotor_pos_control_main },
|
||||
{ "fixedwing_att_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_att_control_main },
|
||||
{ "fixedwing_pos_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_pos_control_main },
|
||||
{ "position_estimator", SCHED_PRIORITY_DEFAULT, 4096, position_estimator_main },
|
||||
{ "attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 2048, attitude_estimator_ekf_main },
|
||||
{ "ms5611", SCHED_PRIORITY_DEFAULT, 2048, ms5611_main },
|
||||
{ "hmc5883", SCHED_PRIORITY_DEFAULT, 4096, hmc5883_main },
|
||||
{ "mpu6000", SCHED_PRIORITY_DEFAULT, 4096, mpu6000_main },
|
||||
{ "bma180", SCHED_PRIORITY_DEFAULT, 2048, bma180_main },
|
||||
{ "l3gd20", SCHED_PRIORITY_DEFAULT, 2048, l3gd20_main },
|
||||
{ "px4io", SCHED_PRIORITY_DEFAULT, 2048, px4io_main },
|
||||
{ "blinkm", SCHED_PRIORITY_DEFAULT, 2048, blinkm_main },
|
||||
{ "tone_alarm", SCHED_PRIORITY_DEFAULT, 2048, tone_alarm_main },
|
||||
{ "adc", SCHED_PRIORITY_DEFAULT, 2048, adc_main },
|
||||
{ "fmu", SCHED_PRIORITY_DEFAULT, 2048, fmu_main },
|
||||
{ "hil", SCHED_PRIORITY_DEFAULT, 2048, hil_main },
|
||||
{ "tests", SCHED_PRIORITY_DEFAULT, 12000, tests_main },
|
||||
{ "sercon", SCHED_PRIORITY_DEFAULT, 2048, sercon_main },
|
||||
{ "serdis", SCHED_PRIORITY_DEFAULT, 2048, serdis_main },
|
|
@ -1,42 +0,0 @@
|
|||
/* List of application entry points, generated during make context. */
|
||||
EXTERN int math_demo_main(int argc, char *argv[]);
|
||||
EXTERN int control_demo_main(int argc, char *argv[]);
|
||||
EXTERN int kalman_demo_main(int argc, char *argv[]);
|
||||
EXTERN int reboot_main(int argc, char *argv[]);
|
||||
EXTERN int perf_main(int argc, char *argv[]);
|
||||
EXTERN int top_main(int argc, char *argv[]);
|
||||
EXTERN int boardinfo_main(int argc, char *argv[]);
|
||||
EXTERN int mixer_main(int argc, char *argv[]);
|
||||
EXTERN int eeprom_main(int argc, char *argv[]);
|
||||
EXTERN int param_main(int argc, char *argv[]);
|
||||
EXTERN int bl_update_main(int argc, char *argv[]);
|
||||
EXTERN int preflight_check_main(int argc, char *argv[]);
|
||||
EXTERN int delay_test_main(int argc, char *argv[]);
|
||||
EXTERN int uorb_main(int argc, char *argv[]);
|
||||
EXTERN int mavlink_main(int argc, char *argv[]);
|
||||
EXTERN int mavlink_onboard_main(int argc, char *argv[]);
|
||||
EXTERN int gps_main(int argc, char *argv[]);
|
||||
EXTERN int commander_main(int argc, char *argv[]);
|
||||
EXTERN int sdlog_main(int argc, char *argv[]);
|
||||
EXTERN int sensors_main(int argc, char *argv[]);
|
||||
EXTERN int ardrone_interface_main(int argc, char *argv[]);
|
||||
EXTERN int multirotor_att_control_main(int argc, char *argv[]);
|
||||
EXTERN int multirotor_pos_control_main(int argc, char *argv[]);
|
||||
EXTERN int fixedwing_att_control_main(int argc, char *argv[]);
|
||||
EXTERN int fixedwing_pos_control_main(int argc, char *argv[]);
|
||||
EXTERN int position_estimator_main(int argc, char *argv[]);
|
||||
EXTERN int attitude_estimator_ekf_main(int argc, char *argv[]);
|
||||
EXTERN int ms5611_main(int argc, char *argv[]);
|
||||
EXTERN int hmc5883_main(int argc, char *argv[]);
|
||||
EXTERN int mpu6000_main(int argc, char *argv[]);
|
||||
EXTERN int bma180_main(int argc, char *argv[]);
|
||||
EXTERN int l3gd20_main(int argc, char *argv[]);
|
||||
EXTERN int px4io_main(int argc, char *argv[]);
|
||||
EXTERN int blinkm_main(int argc, char *argv[]);
|
||||
EXTERN int tone_alarm_main(int argc, char *argv[]);
|
||||
EXTERN int adc_main(int argc, char *argv[]);
|
||||
EXTERN int fmu_main(int argc, char *argv[]);
|
||||
EXTERN int hil_main(int argc, char *argv[]);
|
||||
EXTERN int tests_main(int argc, char *argv[]);
|
||||
EXTERN int sercon_main(int argc, char *argv[]);
|
||||
EXTERN int serdis_main(int argc, char *argv[]);
|
|
@ -154,7 +154,7 @@ adc_measure(unsigned channel)
|
|||
while (!(rSR & ADC_SR_EOC)) {
|
||||
|
||||
/* never spin forever - this will give a bogus result though */
|
||||
if ((hrt_absolute_time() - now) > 1000) {
|
||||
if (hrt_elapsed_time(&now) > 1000) {
|
||||
debug("adc timeout");
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -39,13 +39,11 @@
|
|||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdbool.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/ppm_decode.h>
|
||||
|
||||
//#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
|
||||
|
@ -54,21 +52,18 @@
|
|||
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values);
|
||||
|
||||
void
|
||||
controls_main(void)
|
||||
{
|
||||
struct pollfd fds[2];
|
||||
static perf_counter_t c_gather_dsm;
|
||||
static perf_counter_t c_gather_sbus;
|
||||
static perf_counter_t c_gather_ppm;
|
||||
|
||||
void
|
||||
controls_init(void)
|
||||
{
|
||||
/* DSM input */
|
||||
fds[0].fd = dsm_init("/dev/ttyS0");
|
||||
fds[0].events = POLLIN;
|
||||
dsm_init("/dev/ttyS0");
|
||||
|
||||
/* S.bus input */
|
||||
fds[1].fd = sbus_init("/dev/ttyS2");
|
||||
fds[1].events = POLLIN;
|
||||
|
||||
ASSERT(fds[0].fd >= 0);
|
||||
ASSERT(fds[1].fd >= 0);
|
||||
sbus_init("/dev/ttyS2");
|
||||
|
||||
/* default to a 1:1 input map, all enabled */
|
||||
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
|
||||
|
@ -83,11 +78,13 @@ controls_main(void)
|
|||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
|
||||
for (;;) {
|
||||
/* run this loop at ~100Hz */
|
||||
int result = poll(fds, 2, 10);
|
||||
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
|
||||
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
|
||||
c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
|
||||
}
|
||||
|
||||
ASSERT(result >= 0);
|
||||
void
|
||||
controls_tick() {
|
||||
|
||||
/*
|
||||
* Gather R/C control inputs from supported sources.
|
||||
|
@ -97,22 +94,28 @@ controls_main(void)
|
|||
* other. Don't do that.
|
||||
*/
|
||||
|
||||
perf_begin(c_gather_dsm);
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (dsm_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
perf_end(c_gather_dsm);
|
||||
|
||||
perf_begin(c_gather_sbus);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (sbus_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
perf_end(c_gather_sbus);
|
||||
|
||||
/*
|
||||
* XXX each S.bus frame will cause a PPM decoder interrupt
|
||||
* storm (lots of edges). It might be sensible to actually
|
||||
* disable the PPM decoder completely if we have S.bus signal.
|
||||
*/
|
||||
perf_begin(c_gather_ppm);
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (ppm_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
perf_end(c_gather_ppm);
|
||||
|
||||
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
|
||||
|
||||
|
@ -164,15 +167,12 @@ controls_main(void)
|
|||
|
||||
int16_t scaled = raw;
|
||||
|
||||
/* adjust to zero-relative (-500..500) */
|
||||
scaled -= 1500;
|
||||
/* adjust to zero-relative around center (nominal -500..500) */
|
||||
scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
|
||||
/* scale to fixed-point representation (-10000..10000) */
|
||||
scaled *= 20;
|
||||
|
||||
ASSERT(scaled >= -15000);
|
||||
ASSERT(scaled <= 15000);
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||
scaled = -scaled;
|
||||
|
||||
|
@ -214,7 +214,7 @@ controls_main(void)
|
|||
* If we haven't seen any new control data in 200ms, assume we
|
||||
* have lost input.
|
||||
*/
|
||||
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
|
||||
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
|
||||
rc_input_lost = true;
|
||||
|
||||
/* clear the input-kind flags here */
|
||||
|
@ -276,8 +276,6 @@ controls_main(void)
|
|||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
static bool
|
||||
|
@ -292,7 +290,7 @@ ppm_input(uint16_t *values, uint16_t *num_values)
|
|||
* If we have received a new PPM frame within the last 200ms, accept it
|
||||
* and then invalidate it.
|
||||
*/
|
||||
if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) {
|
||||
if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
|
||||
|
||||
/* PPM data exists, copy it */
|
||||
*num_values = ppm_decoded_channels;
|
||||
|
|
|
@ -231,6 +231,7 @@ dsm_guess_format(bool reset)
|
|||
0x3f, /* 6 channels (DX6) */
|
||||
0x7f, /* 7 channels (DX7) */
|
||||
0xff, /* 8 channels (DX8) */
|
||||
0x1ff, /* 9 channels (DX9, etc.) */
|
||||
0x3ff, /* 10 channels (DX10) */
|
||||
0x3fff /* 18 channels (DX10) */
|
||||
};
|
||||
|
@ -248,18 +249,18 @@ dsm_guess_format(bool reset)
|
|||
|
||||
if ((votes11 == 1) && (votes10 == 0)) {
|
||||
channel_shift = 11;
|
||||
debug("DSM: detected 11-bit format");
|
||||
debug("DSM: 11-bit format");
|
||||
return;
|
||||
}
|
||||
|
||||
if ((votes10 == 1) && (votes11 == 0)) {
|
||||
channel_shift = 10;
|
||||
debug("DSM: detected 10-bit format");
|
||||
debug("DSM: 10-bit format");
|
||||
return;
|
||||
}
|
||||
|
||||
/* call ourselves to reset our state ... we have to try again */
|
||||
debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
|
||||
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
|
||||
dsm_guess_format(true);
|
||||
}
|
||||
|
||||
|
|
|
@ -88,15 +88,19 @@ void
|
|||
mixer_tick(void)
|
||||
{
|
||||
/* check that we are receiving fresh data from the FMU */
|
||||
if ((hrt_absolute_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) {
|
||||
|
||||
/* too long without FMU input, time to go to failsafe */
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
debug("AP RX timeout");
|
||||
}
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
|
||||
|
||||
/* XXX this is questionable - vehicle may not make sense for direct control */
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
} else {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
}
|
||||
|
||||
source = MIX_FAILSAFE;
|
||||
|
@ -157,71 +161,6 @@ mixer_tick(void)
|
|||
r_page_servos[i] = 0;
|
||||
}
|
||||
|
||||
#if 0
|
||||
/* if everything is ok */
|
||||
|
||||
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
|
||||
/* we have recent control data from the FMU */
|
||||
control_count = PX4IO_CONTROL_CHANNELS;
|
||||
control_values = &system_state.fmu_channel_data[0];
|
||||
|
||||
} else if (system_state.rc_channels > 0) {
|
||||
/* when override is on or the fmu is not available, but RC is present */
|
||||
control_count = system_state.rc_channels;
|
||||
|
||||
sched_lock();
|
||||
|
||||
/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
|
||||
rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
|
||||
rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
|
||||
rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
|
||||
rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
|
||||
//rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1];
|
||||
|
||||
/* get the remaining channels, no remapping needed */
|
||||
for (unsigned i = 4; i < system_state.rc_channels; i++) {
|
||||
rc_channel_data[i] = system_state.rc_channel_data[i];
|
||||
}
|
||||
|
||||
/* scale the control inputs */
|
||||
rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) /
|
||||
(float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000;
|
||||
|
||||
if (rc_channel_data[THROTTLE] > 2000) {
|
||||
rc_channel_data[THROTTLE] = 2000;
|
||||
}
|
||||
|
||||
if (rc_channel_data[THROTTLE] < 1000) {
|
||||
rc_channel_data[THROTTLE] = 1000;
|
||||
}
|
||||
|
||||
// lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
|
||||
// (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]),
|
||||
// (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE]));
|
||||
|
||||
control_values = &rc_channel_data[0];
|
||||
sched_unlock();
|
||||
} else {
|
||||
/* we have no control input (no FMU, no RC) */
|
||||
|
||||
// XXX builtin failsafe would activate here
|
||||
control_count = 0;
|
||||
}
|
||||
//lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
|
||||
|
||||
/* this is for multicopters, etc. where manual override does not make sense */
|
||||
} else {
|
||||
/* if the fmu is available whe are good */
|
||||
if (system_state.mixer_fmu_available) {
|
||||
control_count = PX4IO_CONTROL_CHANNELS;
|
||||
control_values = &system_state.fmu_channel_data[0];
|
||||
/* we better shut everything off */
|
||||
} else {
|
||||
control_count = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Decide whether the servos should be armed right now.
|
||||
*
|
||||
|
@ -301,7 +240,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
|
||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||
|
||||
isr_debug(2, "mixer text %u", length);
|
||||
isr_debug(2, "mix txt %u", length);
|
||||
|
||||
if (length < sizeof(px4io_mixdata))
|
||||
return;
|
||||
|
|
|
@ -68,14 +68,8 @@ static struct hrt_call serial_dma_call;
|
|||
volatile uint8_t debug_level = 0;
|
||||
volatile uint32_t i2c_loop_resets = 0;
|
||||
|
||||
struct hrt_call loop_overtime_call;
|
||||
|
||||
// this allows wakeup of the main task via a signal
|
||||
static pid_t daemon_pid;
|
||||
|
||||
|
||||
/*
|
||||
a set of debug buffers to allow us to send debug information from ISRs
|
||||
* a set of debug buffers to allow us to send debug information from ISRs
|
||||
*/
|
||||
|
||||
static volatile uint32_t msg_counter;
|
||||
|
@ -83,17 +77,18 @@ static volatile uint32_t last_msg_counter;
|
|||
static volatile uint8_t msg_next_out, msg_next_in;
|
||||
|
||||
/*
|
||||
* WARNING too large buffers here consume the memory required
|
||||
* WARNING: too large buffers here consume the memory required
|
||||
* for mixer handling. Do not allocate more than 80 bytes for
|
||||
* output.
|
||||
*/
|
||||
#define NUM_MSG 2
|
||||
static char msg[NUM_MSG][50];
|
||||
static char msg[NUM_MSG][40];
|
||||
|
||||
/*
|
||||
add a debug message to be printed on the console
|
||||
* add a debug message to be printed on the console
|
||||
*/
|
||||
void isr_debug(uint8_t level, const char *fmt, ...)
|
||||
void
|
||||
isr_debug(uint8_t level, const char *fmt, ...)
|
||||
{
|
||||
if (level > debug_level) {
|
||||
return;
|
||||
|
@ -107,9 +102,10 @@ void isr_debug(uint8_t level, const char *fmt, ...)
|
|||
}
|
||||
|
||||
/*
|
||||
show all pending debug messages
|
||||
* show all pending debug messages
|
||||
*/
|
||||
static void show_debug_messages(void)
|
||||
static void
|
||||
show_debug_messages(void)
|
||||
{
|
||||
if (msg_counter != last_msg_counter) {
|
||||
uint32_t n = msg_counter - last_msg_counter;
|
||||
|
@ -122,36 +118,9 @@ static void show_debug_messages(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
catch I2C lockups
|
||||
*/
|
||||
static void loop_overtime(void *arg)
|
||||
int
|
||||
user_start(int argc, char *argv[])
|
||||
{
|
||||
debug("RESETTING\n");
|
||||
i2c_loop_resets++;
|
||||
i2c_dump();
|
||||
i2c_reset();
|
||||
hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL);
|
||||
}
|
||||
|
||||
static void wakeup_handler(int signo, siginfo_t *info, void *ucontext)
|
||||
{
|
||||
// nothing to do - we just want poll() to return
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
wakeup the main task using a signal
|
||||
*/
|
||||
void daemon_wakeup(void)
|
||||
{
|
||||
kill(daemon_pid, SIGUSR1);
|
||||
}
|
||||
|
||||
int user_start(int argc, char *argv[])
|
||||
{
|
||||
daemon_pid = getpid();
|
||||
|
||||
/* run C++ ctors before we go any further */
|
||||
up_cxxinitialize();
|
||||
|
||||
|
@ -184,17 +153,8 @@ int user_start(int argc, char *argv[])
|
|||
/* configure the first 8 PWM outputs (i.e. all of them) */
|
||||
up_pwm_servo_init(0xff);
|
||||
|
||||
/* start the flight control signal handler */
|
||||
task_create("FCon",
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
(main_t)controls_main,
|
||||
NULL);
|
||||
|
||||
struct mallinfo minfo = mallinfo();
|
||||
lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
debug("debug_level=%u\n", (unsigned)debug_level);
|
||||
/* initialise the control inputs */
|
||||
controls_init();
|
||||
|
||||
/* start the i2c handler */
|
||||
i2c_init();
|
||||
|
@ -202,39 +162,59 @@ int user_start(int argc, char *argv[])
|
|||
/* add a performance counter for mixing */
|
||||
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
|
||||
|
||||
/*
|
||||
* setup a null handler for SIGUSR1 - we will use this for wakeup from poll()
|
||||
*/
|
||||
struct sigaction sa;
|
||||
memset(&sa, 0, sizeof(sa));
|
||||
sa.sa_sigaction = wakeup_handler;
|
||||
sigfillset(&sa.sa_mask);
|
||||
sigdelset(&sa.sa_mask, SIGUSR1);
|
||||
if (sigaction(SIGUSR1, &sa, NULL) != OK) {
|
||||
debug("Failed to setup SIGUSR1 handler\n");
|
||||
/* add a performance counter for controls */
|
||||
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
|
||||
|
||||
/* and one for measuring the loop rate */
|
||||
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
|
||||
|
||||
struct mallinfo minfo = mallinfo();
|
||||
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
#if 0
|
||||
/* not enough memory, lock down */
|
||||
if (minfo.mxordblk < 500) {
|
||||
lowsyslog("ERR: not enough MEM");
|
||||
bool phase = false;
|
||||
|
||||
if (phase) {
|
||||
LED_AMBER(true);
|
||||
LED_BLUE(false);
|
||||
} else {
|
||||
LED_AMBER(false);
|
||||
LED_BLUE(true);
|
||||
}
|
||||
|
||||
phase = !phase;
|
||||
usleep(300000);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
run the mixer at ~50Hz, using signals to run it early if
|
||||
need be
|
||||
* Run everything in a tight loop.
|
||||
*/
|
||||
|
||||
uint64_t last_debug_time = 0;
|
||||
for (;;) {
|
||||
/*
|
||||
if we are not scheduled for 30ms then reset the I2C bus
|
||||
*/
|
||||
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);
|
||||
/* track the rate at which the loop is running */
|
||||
perf_count(loop_perf);
|
||||
|
||||
/* kick the mixer */
|
||||
perf_begin(mixer_perf);
|
||||
mixer_tick();
|
||||
perf_end(mixer_perf);
|
||||
|
||||
/* kick the control inputs */
|
||||
perf_begin(controls_perf);
|
||||
controls_tick();
|
||||
perf_end(controls_perf);
|
||||
|
||||
/* check for debug activity */
|
||||
show_debug_messages();
|
||||
if (hrt_absolute_time() - last_debug_time > 1000000) {
|
||||
|
||||
/* post debug state at ~1Hz */
|
||||
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
|
||||
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u",
|
||||
(unsigned)debug_level,
|
||||
(unsigned)r_status_flags,
|
||||
|
|
|
@ -105,12 +105,12 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
|
|||
*/
|
||||
struct sys_state_s {
|
||||
|
||||
uint64_t rc_channels_timestamp;
|
||||
volatile uint64_t rc_channels_timestamp;
|
||||
|
||||
/**
|
||||
* Last FMU receive time, in microseconds since system boot
|
||||
*/
|
||||
uint64_t fmu_data_received_time;
|
||||
volatile uint64_t fmu_data_received_time;
|
||||
|
||||
};
|
||||
|
||||
|
@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel);
|
|||
*
|
||||
* Input functions return true when they receive an update from the RC controller.
|
||||
*/
|
||||
extern void controls_main(void);
|
||||
extern void controls_init(void);
|
||||
extern void controls_tick(void);
|
||||
extern int dsm_init(const char *device);
|
||||
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
||||
extern int sbus_init(const char *device);
|
||||
|
@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
|||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
||||
/**
|
||||
* Wake up mixer.
|
||||
*/
|
||||
void daemon_wakeup(void);
|
||||
|
||||
/* send a debug message to the console */
|
||||
extern void isr_debug(uint8_t level, const char *fmt, ...);
|
||||
|
||||
|
|
|
@ -165,8 +165,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
|||
*/
|
||||
uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
/* valid options excluding ENABLE */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
|
||||
/* valid options */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
|
||||
|
||||
/*
|
||||
* PAGE 104 uses r_page_servos.
|
||||
|
@ -203,8 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||
|
||||
// wake up daemon to trigger mixer
|
||||
daemon_wakeup();
|
||||
break;
|
||||
|
||||
/* handle raw PWM input */
|
||||
|
@ -224,8 +222,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
system_state.fmu_data_received_time = hrt_absolute_time();
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||
|
||||
// wake up the main thread to trigger mixer
|
||||
daemon_wakeup();
|
||||
break;
|
||||
|
||||
/* handle setup for servo failsafe values */
|
||||
|
|
|
@ -53,7 +53,7 @@
|
|||
#include "debug.h"
|
||||
|
||||
#define SBUS_FRAME_SIZE 25
|
||||
#define SBUS_INPUT_CHANNELS 18
|
||||
#define SBUS_INPUT_CHANNELS 16
|
||||
|
||||
static int sbus_fd = -1;
|
||||
|
||||
|
@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
|||
}
|
||||
|
||||
/* decode switch channels if data fields are wide enough */
|
||||
if (chancount > 17) {
|
||||
if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
|
||||
chancount = 18;
|
||||
|
||||
/* channel 17 (index 16) */
|
||||
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
|
||||
/* channel 18 (index 17) */
|
||||
|
|
|
@ -1074,36 +1074,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
void
|
||||
Sensors::ppm_poll()
|
||||
{
|
||||
/* fake low-level driver, directly pulling from driver variables */
|
||||
static orb_advert_t rc_input_pub = -1;
|
||||
struct rc_input_values raw;
|
||||
|
||||
raw.timestamp = ppm_last_valid_decode;
|
||||
/* we are accepting this message */
|
||||
_ppm_last_valid = ppm_last_valid_decode;
|
||||
|
||||
/*
|
||||
* 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 (unsigned i = 0; i < ppm_decoded_channels; i++) {
|
||||
raw.values[i] = ppm_buffer[i];
|
||||
}
|
||||
|
||||
raw.channel_count = ppm_decoded_channels;
|
||||
|
||||
/* publish to object request broker */
|
||||
if (rc_input_pub <= 0) {
|
||||
rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
|
||||
bool rc_updated;
|
||||
|
|
|
@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd)
|
|||
* error occurs).
|
||||
*/
|
||||
|
||||
do
|
||||
for (;;)
|
||||
{
|
||||
/* Read one character from the incoming stream */
|
||||
|
||||
|
@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd)
|
|||
{
|
||||
return -errcode;
|
||||
}
|
||||
}
|
||||
}
|
||||
while (nread < 1);
|
||||
|
||||
/* On success, return the character that was read */
|
||||
continue;
|
||||
}
|
||||
|
||||
else if (buffer == '\0')
|
||||
{
|
||||
/* Ignore NUL characters, since they look like EOF to our caller */
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Success, return the character that was read */
|
||||
|
||||
return (int)buffer;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
|
|
|
@ -81,7 +81,7 @@ CONFIGURED_APPS += systemcmds/delay_test
|
|||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
||||
CONFIGURED_APPS += examples/px4_mavlink_debug
|
||||
# CONFIGURED_APPS += examples/px4_mavlink_debug
|
||||
|
||||
# Shared object broker; required by many parts of the system.
|
||||
CONFIGURED_APPS += uORB
|
||||
|
|
|
@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n
|
|||
CONFIG_DISABLE_CLOCK=n
|
||||
CONFIG_DISABLE_POSIX_TIMERS=y
|
||||
CONFIG_DISABLE_PTHREAD=y
|
||||
CONFIG_DISABLE_SIGNALS=n
|
||||
CONFIG_DISABLE_SIGNALS=y
|
||||
CONFIG_DISABLE_MQUEUE=y
|
||||
CONFIG_DISABLE_MOUNTPOINT=y
|
||||
CONFIG_DISABLE_ENVIRON=y
|
||||
CONFIG_DISABLE_POLL=n
|
||||
CONFIG_DISABLE_POLL=y
|
||||
|
||||
#
|
||||
# Misc libc settings
|
||||
|
@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n
|
|||
# timer structures to minimize dynamic allocations. Set to
|
||||
# zero for all dynamic allocations.
|
||||
#
|
||||
CONFIG_MAX_TASKS=8
|
||||
CONFIG_MAX_TASKS=4
|
||||
CONFIG_MAX_TASK_ARGS=4
|
||||
CONFIG_NPTHREAD_KEYS=4
|
||||
CONFIG_NPTHREAD_KEYS=2
|
||||
CONFIG_NFILE_DESCRIPTORS=8
|
||||
CONFIG_NFILE_STREAMS=0
|
||||
CONFIG_NAME_MAX=32
|
||||
CONFIG_NAME_MAX=12
|
||||
CONFIG_STDIO_BUFFER_SIZE=32
|
||||
CONFIG_STDIO_LINEBUFFER=n
|
||||
CONFIG_NUNGET_CHARS=2
|
||||
|
@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n
|
|||
CONFIG_CUSTOM_STACK=n
|
||||
CONFIG_STACK_POINTER=
|
||||
CONFIG_IDLETHREAD_STACKSIZE=1024
|
||||
CONFIG_USERMAIN_STACKSIZE=1024
|
||||
CONFIG_USERMAIN_STACKSIZE=1200
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_PTHREAD_STACK_DEFAULT=1024
|
||||
CONFIG_HEAP_BASE=
|
||||
|
|
Loading…
Reference in New Issue