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

Merge remote-tracking branch 'upstream/master' into hott-esc

Browse Source
This commit is contained in:
Simon Wilks
2013-07-13 01:08:45 +02:00
parent b500cce31e 3b9c306d64
commit 1ccfb623ee
29 changed files with 1018 additions and 801 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_motor_control.c
+1 -1
View File
@@ -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");
src/drivers/drv_gpio.h
+3 -34
View File
@@ -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
src/drivers/drv_pwm_output.h
+9
View File
@@ -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)
src/drivers/gps/ubx.cpp
+390 -478
View File
File diff suppressed because it is too large Load Diff
src/drivers/gps/ubx.h
+12 -49
View File
@@ -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;
};
src/drivers/mpu6000/mpu6000.cpp
+24 -6
View File
@@ -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:
src/drivers/px4fmu/fmu.cpp
+1 -1
View File
@@ -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),
src/drivers/px4io/px4io.cpp
+123 -15
View File
@@ -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'");
}