forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into hott-esc
This commit is contained in:
Simon Wilks
commit
1ccfb623ee
Binary file not shown.
Binary file not shown.
|
|
@ -1,6 +1,12 @@
|
|||
#ifndef _MAVLINK_CONVERSIONS_H_
|
||||
#define _MAVLINK_CONVERSIONS_H_
|
||||
|
||||
/* enable math defines on Windows */
|
||||
#ifdef _MSC_VER
|
||||
#ifndef _USE_MATH_DEFINES
|
||||
#define _USE_MATH_DEFINES
|
||||
#endif
|
||||
#endif
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ int ar_multiplexing_init()
|
|||
{
|
||||
int fd;
|
||||
|
||||
fd = open(GPIO_DEVICE_PATH, 0);
|
||||
fd = open(PX4FMU_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("GPIO: open fail");
|
||||
|
|
|
|||
|
|
@ -63,43 +63,12 @@
|
|||
* they also export GPIO-like things. This is always the GPIOs on the
|
||||
* main board.
|
||||
*/
|
||||
# define GPIO_DEVICE_PATH "/dev/px4fmu"
|
||||
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
|
||||
# define PX4IO_DEVICE_PATH "/dev/px4io"
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
|
||||
/*
|
||||
* PX4IO GPIO numbers.
|
||||
*
|
||||
* XXX note that these are here for reference/future use; currently
|
||||
* there is no good way to wire these up without a common STM32 GPIO
|
||||
* driver, which isn't implemented yet.
|
||||
*/
|
||||
/* outputs */
|
||||
# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */
|
||||
# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */
|
||||
# define GPIO_SERVO_POWER (1<<2) /**< servo power */
|
||||
# define GPIO_RELAY1 (1<<3) /**< relay 1 */
|
||||
# define GPIO_RELAY2 (1<<4) /**< relay 2 */
|
||||
# define GPIO_LED_BLUE (1<<5) /**< blue LED */
|
||||
# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */
|
||||
# define GPIO_LED_SAFETY (1<<7) /**< safety LED */
|
||||
|
||||
/* inputs */
|
||||
# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */
|
||||
# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */
|
||||
# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */
|
||||
|
||||
/**
|
||||
* Default GPIO device - other devices may also support this protocol if
|
||||
* they also export GPIO-like things. This is always the GPIOs on the
|
||||
* main board.
|
||||
*/
|
||||
# define GPIO_DEVICE_PATH "/dev/px4io"
|
||||
|
||||
#endif
|
||||
|
||||
#ifndef GPIO_DEVICE_PATH
|
||||
#ifndef PX4IO_DEVICE_PATH
|
||||
# error No GPIO support for this board.
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm);
|
|||
/** clear the 'ARM ok' bit, which deactivates the safety switch */
|
||||
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
|
||||
|
||||
/** start DSM bind */
|
||||
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
|
||||
|
||||
/** stop DSM bind */
|
||||
#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8)
|
||||
|
||||
/** Power up DSM receiver */
|
||||
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
|
||||
|
||||
/** set a single servo to a specific value */
|
||||
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -3,6 +3,7 @@
|
|||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -51,23 +52,23 @@
|
|||
|
||||
/* MessageIDs (the ones that are used) */
|
||||
#define UBX_MESSAGE_NAV_POSLLH 0x02
|
||||
#define UBX_MESSAGE_NAV_SOL 0x06
|
||||
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
|
||||
//#define UBX_MESSAGE_NAV_DOP 0x04
|
||||
#define UBX_MESSAGE_NAV_SVINFO 0x30
|
||||
#define UBX_MESSAGE_NAV_SOL 0x06
|
||||
#define UBX_MESSAGE_NAV_VELNED 0x12
|
||||
//#define UBX_MESSAGE_RXM_SVSI 0x20
|
||||
#define UBX_MESSAGE_ACK_ACK 0x01
|
||||
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
|
||||
#define UBX_MESSAGE_NAV_SVINFO 0x30
|
||||
#define UBX_MESSAGE_ACK_NAK 0x00
|
||||
#define UBX_MESSAGE_ACK_ACK 0x01
|
||||
#define UBX_MESSAGE_CFG_PRT 0x00
|
||||
#define UBX_MESSAGE_CFG_NAV5 0x24
|
||||
#define UBX_MESSAGE_CFG_MSG 0x01
|
||||
#define UBX_MESSAGE_CFG_RATE 0x08
|
||||
#define UBX_MESSAGE_CFG_NAV5 0x24
|
||||
|
||||
#define UBX_CFG_PRT_LENGTH 20
|
||||
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
|
||||
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
|
||||
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
|
||||
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
|
||||
|
||||
|
|
@ -298,44 +299,6 @@ struct ubx_cfg_msg_rate {
|
|||
// END the structures of the binary packets
|
||||
// ************
|
||||
|
||||
typedef enum {
|
||||
UBX_CONFIG_STATE_PRT = 0,
|
||||
UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
|
||||
UBX_CONFIG_STATE_RATE,
|
||||
UBX_CONFIG_STATE_NAV5,
|
||||
UBX_CONFIG_STATE_MSG_NAV_POSLLH,
|
||||
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
|
||||
UBX_CONFIG_STATE_MSG_NAV_DOP,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SVINFO,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SOL,
|
||||
UBX_CONFIG_STATE_MSG_NAV_VELNED,
|
||||
// UBX_CONFIG_STATE_MSG_RXM_SVSI,
|
||||
UBX_CONFIG_STATE_CONFIGURED
|
||||
} ubx_config_state_t;
|
||||
|
||||
typedef enum {
|
||||
CLASS_UNKNOWN = 0,
|
||||
NAV = 1,
|
||||
RXM = 2,
|
||||
ACK = 3,
|
||||
CFG = 4
|
||||
} ubx_message_class_t;
|
||||
|
||||
typedef enum {
|
||||
//these numbers do NOT correspond to the message id numbers of the ubx protocol
|
||||
ID_UNKNOWN = 0,
|
||||
NAV_POSLLH,
|
||||
NAV_SOL,
|
||||
NAV_TIMEUTC,
|
||||
// NAV_DOP,
|
||||
NAV_SVINFO,
|
||||
NAV_VELNED,
|
||||
// RXM_SVSI,
|
||||
CFG_NAV5,
|
||||
ACK_ACK,
|
||||
ACK_NAK,
|
||||
} ubx_message_id_t;
|
||||
|
||||
typedef enum {
|
||||
UBX_DECODE_UNINIT = 0,
|
||||
UBX_DECODE_GOT_SYNC1,
|
||||
|
|
@ -401,17 +364,17 @@ private:
|
|||
|
||||
int _fd;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
ubx_config_state_t _config_state;
|
||||
bool _configured;
|
||||
bool _waiting_for_ack;
|
||||
uint8_t _clsID_needed;
|
||||
uint8_t _msgID_needed;
|
||||
uint8_t _message_class_needed;
|
||||
uint8_t _message_id_needed;
|
||||
ubx_decode_state_t _decode_state;
|
||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||
unsigned _rx_count;
|
||||
uint8_t _rx_ck_a;
|
||||
uint8_t _rx_ck_b;
|
||||
ubx_message_class_t _message_class;
|
||||
ubx_message_id_t _message_id;
|
||||
uint8_t _message_class;
|
||||
uint8_t _message_id;
|
||||
unsigned _payload_size;
|
||||
uint8_t _disable_cmd_counter;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -272,6 +272,11 @@ private:
|
|||
*/
|
||||
void _set_dlpf_filter(uint16_t frequency_hz);
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz
|
||||
*/
|
||||
void _set_sample_rate(uint16_t desired_sample_rate_hz);
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -378,7 +383,8 @@ MPU6000::init()
|
|||
up_udelay(1000);
|
||||
|
||||
// SAMPLE RATE
|
||||
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
|
||||
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
|
||||
_set_sample_rate(200); // default sample rate = 200Hz
|
||||
usleep(1000);
|
||||
|
||||
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
|
||||
|
|
@ -493,6 +499,18 @@ MPU6000::probe()
|
|||
return -EIO;
|
||||
}
|
||||
|
||||
/*
|
||||
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
|
||||
*/
|
||||
void
|
||||
MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
|
||||
{
|
||||
uint8_t div = 1000 / desired_sample_rate_hz;
|
||||
if(div>200) div=200;
|
||||
if(div<1) div=1;
|
||||
write_reg(MPUREG_SMPLRT_DIV, div-1);
|
||||
}
|
||||
|
||||
/*
|
||||
set the DLPF filter frequency. This affects both accel and gyro.
|
||||
*/
|
||||
|
|
@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
case ACCELIOCSSAMPLERATE:
|
||||
case ACCELIOCGSAMPLERATE:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case ACCELIOCSLOWPASS:
|
||||
case ACCELIOCGLOWPASS:
|
||||
|
|
@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
case GYROIOCSSAMPLERATE:
|
||||
case GYROIOCGSAMPLERATE:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case GYROIOCSLOWPASS:
|
||||
case GYROIOCGLOWPASS:
|
||||
|
|
|
|||
|
|
@ -166,7 +166,7 @@ PX4FMU *g_fmu;
|
|||
} // namespace
|
||||
|
||||
PX4FMU::PX4FMU() :
|
||||
CDev("fmuservo", "/dev/px4fmu"),
|
||||
CDev("fmuservo", PX4FMU_DEVICE_PATH),
|
||||
_mode(MODE_NONE),
|
||||
_pwm_default_rate(50),
|
||||
_pwm_alt_rate(50),
|
||||
|
|
|
|||
|
|
@ -129,6 +129,16 @@ public:
|
|||
*/
|
||||
void print_status();
|
||||
|
||||
inline void set_dsm_vcc_ctl(bool enable)
|
||||
{
|
||||
_dsm_vcc_ctl = enable;
|
||||
};
|
||||
|
||||
inline bool get_dsm_vcc_ctl()
|
||||
{
|
||||
return _dsm_vcc_ctl;
|
||||
};
|
||||
|
||||
private:
|
||||
// XXX
|
||||
unsigned _max_actuators;
|
||||
|
|
@ -172,6 +182,12 @@ private:
|
|||
float _battery_mamphour_total;
|
||||
uint64_t _battery_last_timestamp;
|
||||
|
||||
/**
|
||||
* Relay1 is dedicated to controlling DSM receiver power
|
||||
*/
|
||||
|
||||
bool _dsm_vcc_ctl;
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
*/
|
||||
|
|
@ -313,7 +329,7 @@ PX4IO *g_dev;
|
|||
}
|
||||
|
||||
PX4IO::PX4IO() :
|
||||
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
|
||||
I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
|
||||
_max_actuators(0),
|
||||
_max_controls(0),
|
||||
_max_rc_input(0),
|
||||
|
|
@ -338,7 +354,8 @@ PX4IO::PX4IO() :
|
|||
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
|
||||
_battery_amp_bias(0),
|
||||
_battery_mamphour_total(0),
|
||||
_battery_last_timestamp(0)
|
||||
_battery_last_timestamp(0),
|
||||
_dsm_vcc_ctl(false)
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
|
@ -700,8 +717,6 @@ PX4IO::io_set_control_state()
|
|||
int
|
||||
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
|
||||
{
|
||||
uint16_t regs[_max_actuators];
|
||||
|
||||
if (len > _max_actuators)
|
||||
/* fail with error */
|
||||
return E2BIG;
|
||||
|
|
@ -1271,13 +1286,14 @@ PX4IO::print_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%s%s\n",
|
||||
printf("status 0x%04x%s%s%s%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_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
|
||||
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
|
||||
((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" : ""),
|
||||
|
|
@ -1372,7 +1388,8 @@ PX4IO::print_status()
|
|||
}
|
||||
|
||||
int
|
||||
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||
PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
||||
/* Make it obvious that file * isn't used here */
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
|
|
@ -1424,6 +1441,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
|||
*(unsigned *)arg = _max_actuators;
|
||||
break;
|
||||
|
||||
case DSM_BIND_START:
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
||||
usleep(500000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
|
||||
usleep(1000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
|
||||
usleep(100000);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
|
||||
break;
|
||||
|
||||
case DSM_BIND_STOP:
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
|
||||
usleep(500000);
|
||||
break;
|
||||
|
||||
case DSM_BIND_POWER_UP:
|
||||
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
|
||||
|
||||
/* TODO: we could go lower for e.g. TurboPWM */
|
||||
|
|
@ -1466,18 +1503,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
|||
break;
|
||||
}
|
||||
|
||||
case GPIO_RESET:
|
||||
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
|
||||
case GPIO_RESET: {
|
||||
uint32_t bits = (1 << _max_relays) - 1;
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl)
|
||||
bits &= ~PX4IO_RELAY1;
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
|
||||
break;
|
||||
}
|
||||
|
||||
case GPIO_SET:
|
||||
arg &= ((1 << _max_relays) - 1);
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
|
||||
ret = -EINVAL;
|
||||
else
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
|
||||
break;
|
||||
|
||||
case GPIO_CLEAR:
|
||||
arg &= ((1 << _max_relays) - 1);
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
|
||||
ret = -EINVAL;
|
||||
else
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
|
||||
break;
|
||||
|
||||
case GPIO_GET:
|
||||
|
|
@ -1614,9 +1664,64 @@ start(int argc, char *argv[])
|
|||
errx(1, "driver init failed");
|
||||
}
|
||||
|
||||
int dsm_vcc_ctl;
|
||||
|
||||
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
|
||||
if (dsm_vcc_ctl) {
|
||||
g_dev->set_dsm_vcc_ctl(true);
|
||||
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
|
||||
}
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
bind(int argc, char *argv[])
|
||||
{
|
||||
int pulses;
|
||||
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "px4io must be started first");
|
||||
|
||||
if (!g_dev->get_dsm_vcc_ctl())
|
||||
errx(1, "DSM bind feature not enabled");
|
||||
|
||||
if (argc < 3)
|
||||
errx(0, "needs argument, use dsm2 or dsmx");
|
||||
|
||||
if (!strcmp(argv[2], "dsm2"))
|
||||
pulses = 3;
|
||||
else if (!strcmp(argv[2], "dsmx"))
|
||||
pulses = 7;
|
||||
else
|
||||
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
|
||||
|
||||
/* Open console directly to grab CTRL-C signal */
|
||||
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
|
||||
if (!console)
|
||||
errx(1, "failed opening console");
|
||||
|
||||
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
|
||||
warnx("Press CTRL-C or 'c' when done.");
|
||||
|
||||
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
|
||||
|
||||
for (;;) {
|
||||
usleep(500000L);
|
||||
/* Check if user wants to quit */
|
||||
char c;
|
||||
if (read(console, &c, 1) == 1) {
|
||||
if (c == 0x03 || c == 0x63) {
|
||||
warnx("Done\n");
|
||||
g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
|
||||
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
|
||||
close(console);
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
test(void)
|
||||
{
|
||||
|
|
@ -1626,7 +1731,7 @@ test(void)
|
|||
int direction = 1;
|
||||
int ret;
|
||||
|
||||
fd = open("/dev/px4io", O_WRONLY);
|
||||
fd = open(PX4IO_DEVICE_PATH, O_WRONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed to open device");
|
||||
|
|
@ -1800,7 +1905,7 @@ px4io_main(int argc, char *argv[])
|
|||
* We can cheat and call the driver directly, as it
|
||||
* doesn't reference filp in ioctl()
|
||||
*/
|
||||
g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
|
||||
g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
|
|
@ -1844,7 +1949,7 @@ px4io_main(int argc, char *argv[])
|
|||
/* we can cheat and call the driver directly, as it
|
||||
* doesn't reference filp in ioctl()
|
||||
*/
|
||||
int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
|
||||
int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
|
||||
if (ret != 0) {
|
||||
printf("SET_DEBUG failed - %d\n", ret);
|
||||
exit(1);
|
||||
|
|
@ -1918,6 +2023,9 @@ px4io_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "monitor"))
|
||||
monitor();
|
||||
|
||||
if (!strcmp(argv[1], "bind"))
|
||||
bind(argc, argv);
|
||||
|
||||
out:
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
|
||||
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -661,10 +661,10 @@ int KalmanNav::correctPos()
|
|||
Vector y(6);
|
||||
y(0) = _gps.vel_n_m_s - vN;
|
||||
y(1) = _gps.vel_e_m_s - vE;
|
||||
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
||||
y(5) = double(_sensors.baro_alt_meter) - alt;
|
||||
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(4) = _gps.alt / 1.0e3f - alt;
|
||||
y(5) = _sensors.baro_alt_meter - alt;
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
|
|
@ -698,7 +698,7 @@ int KalmanNav::correctPos()
|
|||
vD += xCorrect(VD);
|
||||
lat += double(xCorrect(LAT));
|
||||
lon += double(xCorrect(LON));
|
||||
alt += double(xCorrect(ALT));
|
||||
alt += xCorrect(ALT);
|
||||
|
||||
// update state covariance
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
|
|
@ -710,7 +710,7 @@ int KalmanNav::correctPos()
|
|||
static int counter = 0;
|
||||
if (beta > _faultPos.get() && (counter % 10 == 0)) {
|
||||
warnx("fault in gps: beta = %8.4f", (double)beta);
|
||||
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
|
||||
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
|
||||
double(y(0) / sqrtf(RPos(0, 0))),
|
||||
double(y(1) / sqrtf(RPos(1, 1))),
|
||||
double(y(2) / sqrtf(RPos(2, 2))),
|
||||
|
|
|
|||
|
|
@ -44,42 +44,42 @@
|
|||
/* Extended Kalman Filter covariances */
|
||||
|
||||
/* gyro process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
|
||||
/* gyro offsets process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
|
||||
|
||||
/* gyro measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
|
||||
/* accelerometer measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
|
||||
|
||||
/* offsets in roll, pitch and yaw of sensor plane and body */
|
||||
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
|
||||
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
h->q0 = param_find("EKF_ATT_V2_Q0");
|
||||
h->q1 = param_find("EKF_ATT_V2_Q1");
|
||||
h->q2 = param_find("EKF_ATT_V2_Q2");
|
||||
h->q3 = param_find("EKF_ATT_V2_Q3");
|
||||
h->q4 = param_find("EKF_ATT_V2_Q4");
|
||||
h->q0 = param_find("EKF_ATT_V3_Q0");
|
||||
h->q1 = param_find("EKF_ATT_V3_Q1");
|
||||
h->q2 = param_find("EKF_ATT_V3_Q2");
|
||||
h->q3 = param_find("EKF_ATT_V3_Q3");
|
||||
h->q4 = param_find("EKF_ATT_V3_Q4");
|
||||
|
||||
h->r0 = param_find("EKF_ATT_V2_R0");
|
||||
h->r1 = param_find("EKF_ATT_V2_R1");
|
||||
h->r2 = param_find("EKF_ATT_V2_R2");
|
||||
h->r3 = param_find("EKF_ATT_V2_R3");
|
||||
h->r0 = param_find("EKF_ATT_V3_R0");
|
||||
h->r1 = param_find("EKF_ATT_V3_R1");
|
||||
h->r2 = param_find("EKF_ATT_V3_R2");
|
||||
h->r3 = param_find("EKF_ATT_V3_R3");
|
||||
|
||||
h->roll_off = param_find("ATT_ROLL_OFFS");
|
||||
h->pitch_off = param_find("ATT_PITCH_OFFS");
|
||||
h->yaw_off = param_find("ATT_YAW_OFFS");
|
||||
h->roll_off = param_find("ATT_ROLL_OFF3");
|
||||
h->pitch_off = param_find("ATT_PITCH_OFF3");
|
||||
h->yaw_off = param_find("ATT_YAW_OFF3");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1389,6 +1389,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
uint64_t start_time = hrt_absolute_time();
|
||||
uint64_t failsave_ll_start_time = 0;
|
||||
|
||||
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
|
||||
bool state_changed = true;
|
||||
bool param_init_forced = true;
|
||||
|
||||
|
|
@ -1828,8 +1829,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
|
||||
|
||||
/* bottom stick position, set altitude hold */
|
||||
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
|
||||
/* bottom stick position, set default */
|
||||
/* this MUST be mapped to extremal position to switch easy in case of emergency */
|
||||
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
|
||||
|
||||
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
|
||||
|
||||
|
|
@ -1837,8 +1839,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
|
||||
|
||||
} else {
|
||||
/* center stick position, set default */
|
||||
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
|
||||
/* center stick position, set altitude hold */
|
||||
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
|
||||
}
|
||||
|
||||
if (current_status.manual_sas_mode != manual_sas_mode) {
|
||||
/* publish SAS mode changes immediately */
|
||||
manual_sas_mode = current_status.manual_sas_mode;
|
||||
state_changed = true;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -1849,8 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
|
||||
) &&
|
||||
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
|
||||
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
|
||||
current_status.flag_control_manual_enabled &&
|
||||
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT &&
|
||||
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd);
|
||||
stick_on_counter = 0;
|
||||
|
|
@ -1862,7 +1872,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* check if left stick is in lower right position --> arm */
|
||||
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
|
||||
if (current_status.flag_control_manual_enabled &&
|
||||
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT &&
|
||||
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd);
|
||||
stick_on_counter = 0;
|
||||
|
|
|
|||
|
|
@ -53,10 +53,12 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <poll.h>
|
||||
#include <drivers/drv_gpio.h>
|
||||
#include <modules/px4iofirmware/protocol.h>
|
||||
|
||||
struct gpio_led_s {
|
||||
struct work_s work;
|
||||
int gpio_fd;
|
||||
bool use_io;
|
||||
int pin;
|
||||
struct vehicle_status_s status;
|
||||
int vehicle_status_sub;
|
||||
|
|
@ -75,51 +77,97 @@ void gpio_led_cycle(FAR void *arg);
|
|||
|
||||
int gpio_led_main(int argc, char *argv[])
|
||||
{
|
||||
int pin = GPIO_EXT_1;
|
||||
|
||||
if (argc < 2) {
|
||||
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
|
||||
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
|
||||
"\t-p\tUse pin:\n"
|
||||
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
|
||||
"\t\t2\tPX4FMU GPIO_EXT2\n"
|
||||
"\t\ta1\tPX4IO ACC1\n"
|
||||
"\t\ta2\tPX4IO ACC2\n"
|
||||
"\t\tr1\tPX4IO RELAY1\n"
|
||||
"\t\tr2\tPX4IO RELAY2");
|
||||
|
||||
} else {
|
||||
|
||||
/* START COMMAND HANDLING */
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
if (gpio_led_started) {
|
||||
errx(1, "already running");
|
||||
}
|
||||
|
||||
bool use_io = false;
|
||||
int pin = GPIO_EXT_1;
|
||||
|
||||
if (argc > 2) {
|
||||
if (!strcmp(argv[1], "-p")) {
|
||||
if (!strcmp(argv[2], "1")) {
|
||||
if (!strcmp(argv[2], "-p")) {
|
||||
if (!strcmp(argv[3], "1")) {
|
||||
use_io = false;
|
||||
pin = GPIO_EXT_1;
|
||||
|
||||
} else if (!strcmp(argv[2], "2")) {
|
||||
} else if (!strcmp(argv[3], "2")) {
|
||||
use_io = false;
|
||||
pin = GPIO_EXT_2;
|
||||
|
||||
} else if (!strcmp(argv[3], "a1")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_ACC1;
|
||||
|
||||
} else if (!strcmp(argv[3], "a2")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_ACC2;
|
||||
|
||||
} else if (!strcmp(argv[3], "r1")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_RELAY1;
|
||||
|
||||
} else if (!strcmp(argv[3], "r2")) {
|
||||
use_io = true;
|
||||
pin = PX4IO_RELAY2;
|
||||
|
||||
} else {
|
||||
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
|
||||
exit(1);
|
||||
errx(1, "unsupported pin: %s", argv[3]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
|
||||
gpio_led_data.use_io = use_io;
|
||||
gpio_led_data.pin = pin;
|
||||
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
|
||||
|
||||
if (ret != 0) {
|
||||
warnx("[gpio_led] Failed to queue work: %d\n", ret);
|
||||
exit(1);
|
||||
errx(1, "failed to queue work: %d", ret);
|
||||
|
||||
} else {
|
||||
gpio_led_started = true;
|
||||
char pin_name[24];
|
||||
|
||||
if (use_io) {
|
||||
if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
|
||||
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
|
||||
|
||||
} else {
|
||||
sprintf(pin_name, "PX4IO RELAY%i", pin);
|
||||
}
|
||||
|
||||
} else {
|
||||
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
|
||||
|
||||
}
|
||||
|
||||
warnx("start, using pin: %s", pin_name);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
/* STOP COMMAND HANDLING */
|
||||
|
||||
} else if (!strcmp(argv[1], "stop")) {
|
||||
gpio_led_started = false;
|
||||
if (gpio_led_started) {
|
||||
gpio_led_started = false;
|
||||
warnx("stop");
|
||||
|
||||
/* INVALID COMMAND */
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
|
||||
|
|
@ -131,15 +179,26 @@ void gpio_led_start(FAR void *arg)
|
|||
{
|
||||
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
|
||||
|
||||
char *gpio_dev;
|
||||
|
||||
if (priv->use_io) {
|
||||
gpio_dev = PX4IO_DEVICE_PATH;
|
||||
} else {
|
||||
gpio_dev = PX4FMU_DEVICE_PATH;
|
||||
}
|
||||
|
||||
/* open GPIO device */
|
||||
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
|
||||
priv->gpio_fd = open(gpio_dev, 0);
|
||||
|
||||
if (priv->gpio_fd < 0) {
|
||||
warnx("[gpio_led] GPIO: open fail\n");
|
||||
// TODO find way to print errors
|
||||
//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
|
||||
gpio_led_started = false;
|
||||
return;
|
||||
}
|
||||
|
||||
/* configure GPIO pin */
|
||||
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
|
||||
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
|
||||
|
||||
/* subscribe to vehicle status topic */
|
||||
|
|
@ -150,11 +209,11 @@ void gpio_led_start(FAR void *arg)
|
|||
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
|
||||
|
||||
if (ret != 0) {
|
||||
warnx("[gpio_led] Failed to queue work: %d\n", ret);
|
||||
// TODO find way to print errors
|
||||
//printf("gpio_led: failed to queue work: %d\n", ret);
|
||||
gpio_led_started = false;
|
||||
return;
|
||||
}
|
||||
|
||||
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
|
||||
}
|
||||
|
||||
void gpio_led_cycle(FAR void *arg)
|
||||
|
|
@ -200,7 +259,6 @@ void gpio_led_cycle(FAR void *arg)
|
|||
|
||||
if (led_state_new) {
|
||||
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
|
||||
|
||||
} else {
|
||||
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
|
||||
}
|
||||
|
|
@ -211,7 +269,12 @@ void gpio_led_cycle(FAR void *arg)
|
|||
if (priv->counter > 5)
|
||||
priv->counter = 0;
|
||||
|
||||
/* repeat cycle at 5 Hz*/
|
||||
if (gpio_led_started)
|
||||
/* repeat cycle at 5 Hz */
|
||||
if (gpio_led_started) {
|
||||
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
|
||||
|
||||
} else {
|
||||
/* switch off LED on stop */
|
||||
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -373,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
|
|||
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
|
||||
|
||||
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
|
||||
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
|
||||
|
||||
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
|
||||
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
|
||||
|
|
|
|||
|
|
@ -95,9 +95,16 @@ controls_tick() {
|
|||
*/
|
||||
|
||||
perf_begin(c_gather_dsm);
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (dsm_updated)
|
||||
uint16_t temp_count = r_raw_rc_count;
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
|
||||
if (dsm_updated) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
r_raw_rc_count = temp_count & 0x7fff;
|
||||
if (temp_count & 0x8000)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
else
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
|
||||
}
|
||||
perf_end(c_gather_dsm);
|
||||
|
||||
perf_begin(c_gather_sbus);
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@
|
|||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/arch.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
|
@ -101,6 +102,41 @@ dsm_init(const char *device)
|
|||
return dsm_fd;
|
||||
}
|
||||
|
||||
void
|
||||
dsm_bind(uint16_t cmd, int pulses)
|
||||
{
|
||||
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
|
||||
|
||||
if (dsm_fd < 0)
|
||||
return;
|
||||
|
||||
switch (cmd) {
|
||||
case dsm_bind_power_down:
|
||||
// power down DSM satellite
|
||||
POWER_RELAY1(0);
|
||||
break;
|
||||
case dsm_bind_power_up:
|
||||
POWER_RELAY1(1);
|
||||
dsm_guess_format(true);
|
||||
break;
|
||||
case dsm_bind_set_rx_out:
|
||||
stm32_configgpio(usart1RxAsOutp);
|
||||
break;
|
||||
case dsm_bind_send_pulses:
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
stm32_gpiowrite(usart1RxAsOutp, false);
|
||||
up_udelay(50);
|
||||
stm32_gpiowrite(usart1RxAsOutp, true);
|
||||
up_udelay(50);
|
||||
}
|
||||
break;
|
||||
case dsm_bind_reinit_uart:
|
||||
// Restore USART rx pin
|
||||
stm32_configgpio(GPIO_USART1_RX);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
|
|
@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
|
|||
|
||||
/*
|
||||
* Iterate the set of sensible sniffed channel sets and see whether
|
||||
* decoding in 10 or 11-bit mode has yielded anything we recognise.
|
||||
* decoding in 10 or 11-bit mode has yielded anything we recognize.
|
||||
*
|
||||
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
|
||||
* stream, we may want to sniff for longer in some cases when we think we
|
||||
|
|
@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
|||
values[channel] = value;
|
||||
}
|
||||
|
||||
if (channel_shift == 11)
|
||||
*num_values |= 0x8000;
|
||||
|
||||
/*
|
||||
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -105,6 +105,7 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
|
|
@ -156,7 +157,22 @@
|
|||
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
|
||||
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
|
||||
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
|
||||
|
||||
/* px4io relay bit definitions */
|
||||
#define PX4IO_RELAY1 (1<<0)
|
||||
#define PX4IO_RELAY2 (1<<1)
|
||||
#define PX4IO_ACC1 (1<<2)
|
||||
#define PX4IO_ACC2 (1<<3)
|
||||
|
||||
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
|
||||
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
|
||||
enum { /* DSM bind states */
|
||||
dsm_bind_power_down = 0,
|
||||
dsm_bind_power_up,
|
||||
dsm_bind_set_rx_out,
|
||||
dsm_bind_send_pulses,
|
||||
dsm_bind_reinit_uart
|
||||
};
|
||||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
|
|
|
|||
|
|
@ -184,6 +184,7 @@ 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 void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
||||
|
||||
|
|
|
|||
|
|
@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
case PX4IO_P_SETUP_RELAYS:
|
||||
value &= PX4IO_P_SETUP_RELAYS_VALID;
|
||||
r_setup_relays = value;
|
||||
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
|
||||
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
|
||||
POWER_ACC1(value & (1 << 2) ? 1 : 0);
|
||||
POWER_ACC2(value & (1 << 3) ? 1 : 0);
|
||||
POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
|
||||
POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
|
||||
POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
|
||||
POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_SET_DEBUG:
|
||||
|
|
@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_DSM:
|
||||
dsm_bind(value & 0x0f, (value >> 4) & 7);
|
||||
break;
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,6 +72,7 @@
|
|||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
|
|
@ -615,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
|
||||
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
|
||||
/* number of messages */
|
||||
const ssize_t fdsc = 18;
|
||||
const ssize_t fdsc = 19;
|
||||
/* Sanity check variable and index */
|
||||
ssize_t fdsc_count = 0;
|
||||
/* file descriptors to wait for */
|
||||
|
|
@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct vehicle_local_position_s local_pos;
|
||||
struct vehicle_local_position_setpoint_s local_pos_sp;
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct vehicle_global_position_setpoint_s global_pos_sp;
|
||||
struct vehicle_gps_position_s gps_pos;
|
||||
struct vehicle_vicon_position_s vicon_pos;
|
||||
struct optical_flow_s flow;
|
||||
|
|
@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
int local_pos_sub;
|
||||
int local_pos_sp_sub;
|
||||
int global_pos_sub;
|
||||
int global_pos_sp_sub;
|
||||
int gps_pos_sub;
|
||||
int vicon_pos_sub;
|
||||
int flow_sub;
|
||||
|
|
@ -689,6 +692,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_ARSP_s log_ARSP;
|
||||
struct log_FLOW_s log_FLOW;
|
||||
struct log_GPOS_s log_GPOS;
|
||||
struct log_GPSP_s log_GPSP;
|
||||
struct log_ESC_s log_ESC;
|
||||
} body;
|
||||
} log_msg = {
|
||||
|
|
@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- GLOBAL POSITION SETPOINT--- */
|
||||
subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
fds[fdsc_count].fd = subs.global_pos_sp_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
fds[fdsc_count].fd = subs.vicon_pos_sub;
|
||||
|
|
@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(GPOS);
|
||||
}
|
||||
|
||||
/* --- GLOBAL POSITION SETPOINT --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
|
||||
log_msg.msg_type = LOG_GPSP_MSG;
|
||||
log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
|
||||
log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
|
||||
log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
|
||||
log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
|
||||
log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
|
||||
log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
|
||||
log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
|
||||
log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
|
||||
log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
|
||||
log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
|
||||
log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
|
||||
log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
|
||||
LOGBUFFER_WRITE_AND_COUNT(GPSP);
|
||||
}
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
|
||||
|
|
|
|||
|
|
@ -149,15 +149,15 @@ struct log_ATTC_s {
|
|||
/* --- STAT - VEHICLE STATE --- */
|
||||
#define LOG_STAT_MSG 10
|
||||
struct log_STAT_s {
|
||||
unsigned char state;
|
||||
unsigned char flight_mode;
|
||||
unsigned char manual_control_mode;
|
||||
unsigned char manual_sas_mode;
|
||||
unsigned char armed;
|
||||
uint8_t state;
|
||||
uint8_t flight_mode;
|
||||
uint8_t manual_control_mode;
|
||||
uint8_t manual_sas_mode;
|
||||
uint8_t armed;
|
||||
float battery_voltage;
|
||||
float battery_current;
|
||||
float battery_remaining;
|
||||
unsigned char battery_warning;
|
||||
uint8_t battery_warning;
|
||||
};
|
||||
|
||||
/* --- RC - RC INPUT CHANNELS --- */
|
||||
|
|
@ -210,13 +210,29 @@ struct log_GPOS_s {
|
|||
float vel_d;
|
||||
};
|
||||
|
||||
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
|
||||
#define LOG_GPSP_MSG 17
|
||||
struct log_GPSP_s {
|
||||
uint8_t altitude_is_relative;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
float altitude;
|
||||
float yaw;
|
||||
float loiter_radius;
|
||||
int8_t loiter_direction;
|
||||
uint8_t nav_cmd;
|
||||
float param1;
|
||||
float param2;
|
||||
float param3;
|
||||
float param4;
|
||||
};
|
||||
|
||||
/* --- ESC - ESC STATE --- */
|
||||
#define LOG_ESC_MSG 64
|
||||
#define LOG_ESC_MSG 18
|
||||
struct log_ESC_s {
|
||||
uint16_t counter;
|
||||
uint8_t esc_count;
|
||||
uint8_t esc_connectiontype;
|
||||
|
||||
uint8_t esc_num;
|
||||
uint16_t esc_address;
|
||||
uint16_t esc_version;
|
||||
|
|
@ -227,6 +243,7 @@ struct log_ESC_s {
|
|||
float esc_setpoint;
|
||||
uint16_t esc_setpoint_raw;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
/* construct list of all message formats */
|
||||
|
|
@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
|
||||
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
|
|||
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
|
||||
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
|
||||
|
||||
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
|
||||
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
|
||||
|
|
|
|||
|
|
@ -230,6 +230,8 @@ private:
|
|||
float rc_scale_flaps;
|
||||
|
||||
float battery_voltage_scaling;
|
||||
|
||||
int rc_rl1_DSM_VCC_control;
|
||||
} _parameters; /**< local copies of interesting parameters */
|
||||
|
||||
struct {
|
||||
|
|
@ -278,6 +280,8 @@ private:
|
|||
param_t rc_scale_flaps;
|
||||
|
||||
param_t battery_voltage_scaling;
|
||||
|
||||
param_t rc_rl1_DSM_VCC_control;
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
|
|
@ -514,6 +518,9 @@ Sensors::Sensors() :
|
|||
|
||||
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
||||
|
||||
/* DSM VCC relay control */
|
||||
_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
|
@ -730,6 +737,11 @@ Sensors::parameters_update()
|
|||
warnx("Failed updating voltage scaling param");
|
||||
}
|
||||
|
||||
/* relay 1 DSM VCC control */
|
||||
if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
|
||||
warnx("Failed updating relay 1 DSM VCC control");
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
|
|||
|
||||
void cpuload_initialize_once()
|
||||
{
|
||||
// if (!system_load.initialized)
|
||||
// {
|
||||
system_load.start_time = hrt_absolute_time();
|
||||
int i;
|
||||
|
||||
|
|
@ -80,27 +78,29 @@ void cpuload_initialize_once()
|
|||
system_load.tasks[i].valid = false;
|
||||
}
|
||||
|
||||
system_load.total_count = 0;
|
||||
|
||||
uint64_t now = hrt_absolute_time();
|
||||
|
||||
/* initialize idle thread statically */
|
||||
system_load.tasks[0].start_time = now;
|
||||
system_load.tasks[0].total_runtime = 0;
|
||||
system_load.tasks[0].curr_start_time = 0;
|
||||
system_load.tasks[0].tcb = sched_gettcb(0);
|
||||
system_load.tasks[0].valid = true;
|
||||
system_load.total_count++;
|
||||
int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init"
|
||||
|
||||
/* initialize init thread statically */
|
||||
system_load.tasks[1].start_time = now;
|
||||
system_load.tasks[1].total_runtime = 0;
|
||||
system_load.tasks[1].curr_start_time = 0;
|
||||
system_load.tasks[1].tcb = sched_gettcb(1);
|
||||
system_load.tasks[1].valid = true;
|
||||
/* count init thread */
|
||||
system_load.total_count++;
|
||||
// }
|
||||
#ifdef CONFIG_PAGING
|
||||
static_tasks_count++; // include paging thread in initialization
|
||||
#endif /* CONFIG_PAGING */
|
||||
#if CONFIG_SCHED_WORKQUEUE
|
||||
static_tasks_count++; // include high priority work0 thread in initialization
|
||||
#endif /* CONFIG_SCHED_WORKQUEUE */
|
||||
#if CONFIG_SCHED_LPWORK
|
||||
static_tasks_count++; // include low priority work1 thread in initialization
|
||||
#endif /* CONFIG_SCHED_WORKQUEUE */
|
||||
|
||||
// perform static initialization of "system" threads
|
||||
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
|
||||
{
|
||||
system_load.tasks[system_load.total_count].start_time = now;
|
||||
system_load.tasks[system_load.total_count].total_runtime = 0;
|
||||
system_load.tasks[system_load.total_count].curr_start_time = 0;
|
||||
system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs
|
||||
system_load.tasks[system_load.total_count].valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
void sched_note_start(FAR struct tcb_s *tcb)
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
|
|||
float altitude; /**< altitude in meters */
|
||||
float yaw; /**< in radians NED -PI..+PI */
|
||||
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
|
||||
uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
|
||||
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
|
||||
enum NAV_CMD nav_cmd; /**< true if loitering is enabled */
|
||||
float param1;
|
||||
float param2;
|
||||
|
|
|
|||
|
|
@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[])
|
|||
int fd;
|
||||
int ret = 0;
|
||||
|
||||
fd = open(GPIO_DEVICE_PATH, 0);
|
||||
fd = open(PX4IO_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("GPIO: open fail\n");
|
||||
|
|
|
|||
|
|
@ -51,19 +51,46 @@
|
|||
#include <systemlib/cpuload.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define CL "\033[K" // clear line
|
||||
|
||||
/**
|
||||
* Start the top application.
|
||||
*/
|
||||
__EXPORT int top_main(int argc, char *argv[]);
|
||||
__EXPORT int top_main(void);
|
||||
|
||||
extern struct system_load_s system_load;
|
||||
|
||||
bool top_sigusr1_rcvd = false;
|
||||
|
||||
int top_main(int argc, char *argv[])
|
||||
static const char *
|
||||
tstate_name(const tstate_t s)
|
||||
{
|
||||
int t;
|
||||
switch (s) {
|
||||
case TSTATE_TASK_INVALID: return "init";
|
||||
|
||||
case TSTATE_TASK_PENDING: return "PEND";
|
||||
case TSTATE_TASK_READYTORUN: return "READY";
|
||||
case TSTATE_TASK_RUNNING: return "RUN";
|
||||
|
||||
case TSTATE_TASK_INACTIVE: return "inact";
|
||||
case TSTATE_WAIT_SEM: return "w:sem";
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
case TSTATE_WAIT_SIG: return "w:sig";
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
|
||||
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
|
||||
#endif
|
||||
|
||||
default:
|
||||
return "ERROR";
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
top_main(void)
|
||||
{
|
||||
uint64_t total_user_time = 0;
|
||||
|
||||
int running_count = 0;
|
||||
|
|
@ -75,7 +102,7 @@ int top_main(int argc, char *argv[])
|
|||
uint64_t last_times[CONFIG_MAX_TASKS];
|
||||
float curr_loads[CONFIG_MAX_TASKS];
|
||||
|
||||
for (t = 0; t < CONFIG_MAX_TASKS; t++)
|
||||
for (int t = 0; t < CONFIG_MAX_TASKS; t++)
|
||||
last_times[t] = 0;
|
||||
|
||||
float interval_time_ms_inv = 0.f;
|
||||
|
|
@ -83,16 +110,16 @@ int top_main(int argc, char *argv[])
|
|||
/* Open console directly to grab CTRL-C signal */
|
||||
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
|
||||
|
||||
while (true)
|
||||
// for (t = 0; t < 10; t++)
|
||||
{
|
||||
int i;
|
||||
/* clear screen */
|
||||
printf("\033[2J");
|
||||
|
||||
uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
|
||||
unsigned int curr_time_s = curr_time_ms / 1000LLU;
|
||||
for (;;) {
|
||||
int i;
|
||||
uint64_t curr_time_us;
|
||||
uint64_t idle_time_us;
|
||||
|
||||
uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
|
||||
unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
|
||||
curr_time_us = hrt_absolute_time();
|
||||
idle_time_us = system_load.tasks[0].total_runtime;
|
||||
|
||||
if (new_time > interval_start_time)
|
||||
interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
|
||||
|
|
@ -102,7 +129,38 @@ int top_main(int argc, char *argv[])
|
|||
total_user_time = 0;
|
||||
|
||||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
|
||||
uint64_t interval_runtime;
|
||||
|
||||
if (system_load.tasks[i].valid) {
|
||||
switch (system_load.tasks[i].tcb->task_state) {
|
||||
case TSTATE_TASK_PENDING:
|
||||
case TSTATE_TASK_READYTORUN:
|
||||
case TSTATE_TASK_RUNNING:
|
||||
running_count++;
|
||||
break;
|
||||
|
||||
case TSTATE_TASK_INVALID:
|
||||
case TSTATE_TASK_INACTIVE:
|
||||
case TSTATE_WAIT_SEM:
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
case TSTATE_WAIT_SIG:
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
case TSTATE_WAIT_MQNOTEMPTY:
|
||||
case TSTATE_WAIT_MQNOTFULL:
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
case TSTATE_WAIT_PAGEFILL:
|
||||
#endif
|
||||
blocked_count++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
|
||||
system_load.tasks[i].total_runtime > last_times[i])
|
||||
? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
|
||||
: 0;
|
||||
|
||||
last_times[i] = system_load.tasks[i].total_runtime;
|
||||
|
||||
|
|
@ -111,7 +169,6 @@ int top_main(int argc, char *argv[])
|
|||
|
||||
if (i > 0)
|
||||
total_user_time += interval_runtime;
|
||||
|
||||
} else
|
||||
curr_loads[i] = 0;
|
||||
}
|
||||
|
|
@ -119,127 +176,99 @@ int top_main(int argc, char *argv[])
|
|||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
|
||||
if (system_load.tasks[i].tcb->pid == 0) {
|
||||
float idle = curr_loads[0];
|
||||
float task_load = (float)(total_user_time) * interval_time_ms_inv;
|
||||
float idle;
|
||||
float task_load;
|
||||
float sched_load;
|
||||
|
||||
if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
|
||||
idle = curr_loads[0];
|
||||
task_load = (float)(total_user_time) * interval_time_ms_inv;
|
||||
|
||||
float sched_load = 1.f - idle - task_load;
|
||||
/* this can happen if one tasks total runtime was not computed
|
||||
correctly by the scheduler instrumentation TODO */
|
||||
if (task_load > (1.f - idle))
|
||||
task_load = (1.f - idle);
|
||||
|
||||
sched_load = 1.f - idle - task_load;
|
||||
|
||||
/* print system information */
|
||||
printf("\033[H"); /* cursor home */
|
||||
printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
|
||||
printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
|
||||
printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
|
||||
printf("\033[H"); /* move cursor home and clear screen */
|
||||
printf(CL "Processes: %d total, %d running, %d sleeping\n",
|
||||
system_load.total_count,
|
||||
running_count,
|
||||
blocked_count);
|
||||
printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
|
||||
(double)(task_load * 100.f),
|
||||
(double)(sched_load * 100.f),
|
||||
(double)(idle * 100.f));
|
||||
printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
|
||||
(double)curr_time_us / 1000000.d,
|
||||
(double)idle_time_us / 1000000.d);
|
||||
|
||||
/* 34 chars command name length (32 chars plus two spaces) */
|
||||
char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
|
||||
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
|
||||
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
|
||||
/* header for task list */
|
||||
printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
|
||||
"PID",
|
||||
CONFIG_TASK_NAME_SIZE, "COMMAND",
|
||||
"CPU(ms)",
|
||||
"CPU(%)",
|
||||
"USED/STACK",
|
||||
"PRIO(BASE)",
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
|
||||
"TSLICE"
|
||||
#else
|
||||
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
|
||||
#endif
|
||||
|
||||
} else {
|
||||
enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
|
||||
|
||||
if (task_state == TSTATE_TASK_PENDING ||
|
||||
task_state == TSTATE_TASK_READYTORUN ||
|
||||
task_state == TSTATE_TASK_RUNNING) {
|
||||
running_count++;
|
||||
}
|
||||
|
||||
if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */
|
||||
task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
|| task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
|| task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */
|
||||
|| task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
|| task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */
|
||||
#endif
|
||||
) {
|
||||
blocked_count++;
|
||||
}
|
||||
|
||||
char spaces[CONFIG_TASK_NAME_SIZE + 2];
|
||||
|
||||
/* count name len */
|
||||
int namelen = 0;
|
||||
|
||||
while (namelen < CONFIG_TASK_NAME_SIZE) {
|
||||
if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
|
||||
|
||||
namelen++;
|
||||
}
|
||||
|
||||
int s = 0;
|
||||
|
||||
for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
|
||||
spaces[s] = ' ';
|
||||
}
|
||||
|
||||
spaces[s] = '\0';
|
||||
|
||||
char *runtime_spaces = " ";
|
||||
|
||||
if ((system_load.tasks[i].total_runtime / 1000) < 99) {
|
||||
runtime_spaces = "";
|
||||
}
|
||||
|
||||
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
||||
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
unsigned stack_free = 0;
|
||||
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
|
||||
while (stack_free < stack_size) {
|
||||
if (*stack_sweeper++ != 0xff)
|
||||
break;
|
||||
|
||||
stack_free++;
|
||||
}
|
||||
|
||||
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 4u / % 4u",
|
||||
(int)system_load.tasks[i].tcb->pid,
|
||||
system_load.tasks[i].tcb->name,
|
||||
spaces,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
runtime_spaces,
|
||||
(int)(curr_loads[i] * 100),
|
||||
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
|
||||
stack_size - stack_free,
|
||||
stack_size);
|
||||
/* Print scheduling info with RR time slice */
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
/* Print scheduling info without time slice*/
|
||||
printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
|
||||
"STATE"
|
||||
#endif
|
||||
);
|
||||
}
|
||||
|
||||
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
||||
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
unsigned stack_free = 0;
|
||||
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
|
||||
while (stack_free < stack_size) {
|
||||
if (*stack_sweeper++ != 0xff)
|
||||
break;
|
||||
|
||||
stack_free++;
|
||||
}
|
||||
|
||||
printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
|
||||
system_load.tasks[i].tcb->pid,
|
||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
(int)(curr_loads[i] * 100),
|
||||
(int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
|
||||
stack_size - stack_free,
|
||||
stack_size,
|
||||
system_load.tasks[i].tcb->sched_priority,
|
||||
system_load.tasks[i].tcb->base_priority);
|
||||
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
/* print scheduling info with RR time slice */
|
||||
printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
// print task state instead
|
||||
printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
|
||||
fflush(stdout);
|
||||
|
||||
interval_start_time = new_time;
|
||||
|
||||
char c;
|
||||
|
||||
/* Sleep 200 ms waiting for user input four times */
|
||||
/* Sleep 200 ms waiting for user input five times ~ 1s */
|
||||
/* XXX use poll ... */
|
||||
for (int k = 0; k < 4; k++) {
|
||||
for (int k = 0; k < 5; k++) {
|
||||
char c;
|
||||
|
||||
if (read(console, &c, 1) == 1) {
|
||||
if (c == 0x03 || c == 0x63) {
|
||||
printf("Abort\n");
|
||||
switch (c) {
|
||||
case 0x03: // ctrl-c
|
||||
case 0x1b: // esc
|
||||
case 'c':
|
||||
case 'q':
|
||||
close(console);
|
||||
return OK;
|
||||
/* not reached */
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue