fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 18:13:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into PR_RCLossDur2

Browse Source
This commit is contained in:
philipoe
2014-11-20 17:26:27 +01:00
parent 5c34f03c4e 97a1410ec9
commit ec165b3f7e
142 changed files with 2249 additions and 763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+1 -1
View File
@@ -147,7 +147,7 @@ Airspeed::init()
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. uORB started?");
warnx("uORB started?");
}
ret = OK;
src/drivers/ardrone_interface/ardrone_interface.c
+14 -22
View File
@@ -89,8 +89,8 @@ static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
warnx("%s\n", reason);
warnx("usage: {start|stop|status} [-d <UART>]\n\n");
exit(1);
}
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ardrone_interface already running\n");
warnx("already running\n");
/* this is not an error */
exit(0);
}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tardrone_interface is running\n");
warnx("running");
} else {
printf("\tardrone_interface not started\n");
warnx("not started");
}
exit(0);
}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
warnx("ERR: tcsetattr: %s", uart_name);
close(uart);
return -1;
}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
char *device = "/dev/ttyS1";
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
struct termios uart_config_original;
if (motor_test_mode) {
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
warnx("setting 10 %% thrust.\n");
}
/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
warnx("No UART, exiting.");
thread_running = false;
exit(ERROR);
}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
warnx("write fail");
thread_running = false;
exit(ERROR);
}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
warnx("ERR: tcsetattr");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
src/drivers/ardrone_interface/ardrone_motor_control.c
+1 -1
View File
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
warnx("Failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
src/drivers/boards/aerocore/module.mk
+2
View File
@@ -6,3 +6,5 @@ SRCS = aerocore_init.c \
aerocore_pwm_servo.c \
aerocore_spi.c \
aerocore_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4fmu-v1/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4fmu-v2/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu2_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4io-v1/module.mk
+2
View File
@@ -4,3 +4,5 @@
SRCS = px4io_init.c \
px4io_pwm_servo.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4io-v2/board_config.h
+1
View File
@@ -77,6 +77,7 @@
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
/* Safety switch button *******************************************************/
src/drivers/boards/px4io-v2/module.mk
+2
View File
@@ -4,3 +4,5 @@
SRCS = px4iov2_init.c \
px4iov2_pwm_servo.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4io-v2/px4iov2_init.c
+1
View File
@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_LED4);
stm32_configgpio(GPIO_BTN_SAFETY);
src/drivers/device/device.h
+2 -1
View File
@@ -130,7 +130,8 @@ public:
enum DeviceBusType {
DeviceBusType_UNKNOWN = 0,
DeviceBusType_I2C = 1,
DeviceBusType_SPI = 2
DeviceBusType_SPI = 2,
DeviceBusType_UAVCAN = 3,
};
/*
src/drivers/drv_pwm_output.h
+32
View File
@@ -117,6 +117,23 @@ struct pwm_output_values {
unsigned channel_count;
};
/**
* RC config values for a channel
*
* This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
* param_get() dependency
*/
struct pwm_output_rc_config {
uint8_t channel;
uint16_t rc_min;
uint16_t rc_trim;
uint16_t rc_max;
uint16_t rc_dz;
uint16_t rc_assignment;
bool rc_reverse;
};
/*
* ORB tag for PWM outputs.
*/
@@ -213,6 +230,21 @@ ORB_DECLARE(output_pwm);
/** make failsafe non-recoverable (termination) if it occurs */
#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25)
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26)
/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27)
/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28)
/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29)
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30)
/*
*
*
src/drivers/frsky_telemetry/frsky_telemetry.c
+3 -6
View File
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
static const speed_t speed = B9600;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
warnx("ERR: %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
}
/* Print welcome text */
warnx("FrSky telemetry interface starting...");
/* Open UART */
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
src/drivers/hil/hil.cpp
-2
View File
@@ -442,8 +442,6 @@ HIL::task_main()
/* make sure servos are off */
// up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
src/drivers/hmc5883/hmc5883.cpp
+15 -34
View File
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 1");
goto out;
}
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 2");
goto out;
}
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("failed to set new scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
}
} else {
warnx("mag scale calibration failed.");
}
return ret;
src/drivers/hmc5883/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = hmc5883.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/hott/comms.cpp
+4 -4
View File
@@ -55,7 +55,7 @@ open_uart(const char *device)
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", device);
err(1, "ERR: opening %s", device);
}
/* Back up the original uart configuration to restore it after exit */
@@ -63,7 +63,7 @@ open_uart(const char *device)
struct termios uart_config_original;
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
err(1, "ERR: %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
err(1, "ERR: %s (tcsetattr)", device);
}
/* Activate single wire mode */
src/drivers/hott/hott_sensors/hott_sensors.cpp
+3 -3
View File
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("deamon already running");
warnx("already running");
exit(0);
}
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("daemon is running");
warnx("is running");
} else {
warnx("daemon not started");
warnx("not started");
}
exit(0);
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+3 -3
View File
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("deamon already running");
warnx("already running");
exit(0);
}
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("daemon is running");
warnx("is running");
} else {
warnx("daemon not started");
warnx("not started");
}
exit(0);
src/drivers/l3gd20/l3gd20.cpp
+8 -1
View File
@@ -176,6 +176,7 @@ static const int ERROR = -1;
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
@@ -856,7 +857,7 @@ L3GD20::measure()
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
@@ -930,6 +931,8 @@ L3GD20::measure()
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@@ -938,6 +941,8 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
@@ -1091,9 +1096,11 @@ test()
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("temp: \t%d\tC", (int)g_report.temperature);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
src/drivers/l3gd20/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = l3gd20.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/ll40ls/ll40ls.cpp
+50 -3
View File
@@ -73,15 +73,19 @@
/* Configuration Constants */
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION
#define LL40LS_BASEADDR 0x42 /* 7-bit address */
#define LL40LS_BASEADDR 0x62 /* 7-bit address */
#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */
#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int"
#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext"
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */
#define LL40LS_WHO_AM_I_REG 0x11
#define LL40LS_WHO_AM_I_REG_VAL 0xCA
#define LL40LS_SIGNAL_STRENGTH_REG 0x5b
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
@@ -117,6 +121,7 @@ public:
protected:
virtual int probe();
virtual int read_reg(uint8_t reg, uint8_t &val);
private:
float _min_distance;
@@ -210,7 +215,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) :
_bus(bus)
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
_retries = 3;
// enable debug() calls
_debug_enabled = false;
@@ -277,9 +282,51 @@ out:
return ret;
}
int
LL40LS::read_reg(uint8_t reg, uint8_t &val)
{
return transfer(&reg, 1, &val, 1);
}
int
LL40LS::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i=0; i<sizeof(addresses); i++) {
uint8_t val=0, who_am_i=0;
// set the I2C bus address
set_address(addresses[i]);
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a WHO_AM_I
goto ok;
}
if (read_reg(LL40LS_SIGNAL_STRENGTH_REG, val) == OK && val != 0) {
// very likely to be a ll40ls. px4flow does not
// respond to this
goto ok;
}
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
(unsigned)who_am_i,
LL40LS_WHO_AM_I_REG_VAL,
(unsigned)val);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
// start a measurement
return measure();
}
src/drivers/lsm303d/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = lsm303d.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/meas_airspeed/meas_airspeed.cpp
+1 -1
View File
@@ -519,7 +519,7 @@ test()
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
errx(1, "timed out");
}
/* now go get it */
src/drivers/mpu6000/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = mpu6000.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/mpu6000/mpu6000.cpp
+11
View File
@@ -229,6 +229,7 @@ private:
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _good_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -456,6 +458,7 @@ MPU6000::~MPU6000()
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
perf_free(_good_transfers);
}
int
@@ -1279,8 +1282,14 @@ MPU6000::measure()
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
/*
@@ -1399,6 +1408,8 @@ MPU6000::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_good_transfers);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
src/drivers/px4flow/px4flow.cpp
+115 -74
View File
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW()
stop();
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
}
int
@@ -222,22 +223,25 @@ PX4FLOW::init()
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
}
/* get a publish handle on the px4flow topic */
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0)
debug("failed to create px4flow object. Did you start uOrb?");
if (_px4flow_topic < 0) {
warnx("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
@@ -249,6 +253,17 @@ out:
int
PX4FLOW::probe()
{
uint8_t val[22];
// to be sure this is not a ll40ls Lidar (which can also be on
// 0x42) we check if a 22 byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
if (transfer(nullptr, 0, &val[0], 22) != OK) {
return -EIO;
}
// that worked, so start a measurement cycle
return measure();
}
@@ -260,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
@@ -283,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@@ -298,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
@@ -314,25 +332,29 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
@@ -355,8 +377,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
@@ -417,13 +440,11 @@ PX4FLOW::measure()
uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d");
return ret;
}
ret = OK;
return ret;
@@ -435,15 +456,14 @@ PX4FLOW::collect()
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0)
{
log("error reading from sensor: %d", ret);
if (ret < 0) {
debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@@ -467,12 +487,12 @@ PX4FLOW::collect()
int16_t gdist = val[21] << 8 | val[20];
struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
report.flow_raw_y= val[5] << 8 | val[4];
report.ground_distance_m =float(gdist)/1000.0f;
report.quality= val[10];
report.flow_comp_x_m = float(flowcx) / 1000.0f;
report.flow_comp_y_m = float(flowcy) / 1000.0f;
report.flow_raw_x = val[3] << 8 | val[2];
report.flow_raw_y = val[5] << 8 | val[4];
report.ground_distance_m = float(gdist) / 1000.0f;
report.quality = val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time();
@@ -509,11 +529,13 @@ PX4FLOW::start()
true,
true,
true,
SUBSYSTEM_TYPE_OPTICALFLOW};
SUBSYSTEM_TYPE_OPTICALFLOW
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
@@ -541,7 +563,7 @@ PX4FLOW::cycle()
/* perform collection */
if (OK != collect()) {
log("collection error");
debug("collection error");
/* restart the measurement state machine */
start();
return;
@@ -567,8 +589,9 @@ PX4FLOW::cycle()
}
/* measurement phase */
if (OK != measure())
log("measure error");
if (OK != measure()) {
debug("measure error");
}
/* next phase is collection */
_collect_phase = true;
@@ -619,33 +642,37 @@ start()
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init())
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
@@ -658,15 +685,14 @@ fail:
*/
void stop()
{
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
else
{
} else {
errx(1, "driver not running");
}
exit(0);
}
@@ -684,14 +710,17 @@ test()
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
// err(1, "immediate read failed");
{
warnx("immediate read failed");
}
warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -700,8 +729,9 @@ test()
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
@@ -712,14 +742,16 @@ test()
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
@@ -740,14 +772,17 @@ reset()
{
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
@@ -758,8 +793,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
@@ -775,32 +811,37 @@ px4flow_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
if (!strcmp(argv[1], "start")) {
px4flow::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
px4flow::stop();
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
px4flow::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
px4flow::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
px4flow::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
px4flow::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
src/drivers/px4fmu/fmu.cpp
+1
View File
@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
// these are no-ops, as no safety switch
break;
src/drivers/px4fmu/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/px4io/module.mk
+2
View File
@@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/px4io/px4io.cpp
+81 -17
View File
@@ -1245,30 +1245,43 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
/* ROLL */
param_get(param_find("RC_MAP_ROLL"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 0;
}
/* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 1;
}
/* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 2;
}
/* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 3;
}
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
/* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 4;
}
/* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
/*
* Iterate all possible RC inputs.
@@ -1644,10 +1657,6 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* data we are going to fetch */
actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
@@ -2048,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2058,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2271,6 +2281,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_SAFETY_ON:
/* force safety switch on */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
@@ -2293,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
/* control whether override on FMU failure is
immediate or waits for override threshold on mode
switch */
if (arg == 0) {
/* clear override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
} else {
/* set override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2554,6 +2582,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_RC_CONFIG: {
/* enable setting of RC configuration without relying
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= _max_actuators) {
/* fail with error */
return E2BIG;
}
/* copy values to registers in IO */
uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min;
regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim;
regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max;
regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz;
regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
if (config->rc_reverse) {
regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
}
ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
break;
}
case PWM_SERVO_SET_OVERRIDE_OK:
/* set the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
break;
case PWM_SERVO_CLEAR_OVERRIDE_OK:
/* clear the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
break;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
src/drivers/stm32/adc/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = adc
SRCS = adc.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/drivers/stm32/module.mk
+2
View File
@@ -41,3 +41,5 @@ SRCS = drv_hrt.c \
drv_pwm_servo.c
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/drivers/stm32/tone_alarm/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm
SRCS = tone_alarm.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/lib/conversion/rotation.h
+3 -1
View File
@@ -74,6 +74,7 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_ROLL_270_YAW_270 = 26,
ROTATION_MAX
};
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
{ 0, 270, 0 },
{270, 0, 270 }
};
/**
src/lib/external_lgpl/tecs/tecs.cpp
+10 -12
View File
@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed) {
_throttle_dem_unc = 1.0f;
_throttle_dem = 1.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
// Calculate PD + FF throttle
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value dduring climbout
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
@@ -363,10 +361,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
_throttle_dem = ff_throttle;
}
}
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
}
}
void TECS::_detect_bad_descent(void)
src/lib/external_lgpl/tecs/tecs.h
-3
View File
@@ -345,9 +345,6 @@ private:
// climbout mode
bool _climbOutDem;
// throttle demand before limiting
float _throttle_dem_unc;
// pitch demand before limiting
float _pitch_dem_unc;
src/modules/bottle_drop/bottle_drop.cpp
+10 -11
View File
@@ -223,7 +223,7 @@ BottleDrop::start()
_main_task = task_spawn_cmd("bottle_drop",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
2048,
1500,
(main_t)&BottleDrop::task_main_trampoline,
nullptr);
@@ -283,7 +283,6 @@ BottleDrop::drop()
// force the door open if we have to
if (_doors_opened == 0) {
open_bay();
warnx("bay not ready, forced open");
}
while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
mavlink_log_critical(_mavlink_fd, "drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
mavlink_log_critical(_mavlink_fd, "opening bay");
} else {
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
mavlink_log_critical(_mavlink_fd, "closing bay");
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
break;
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
mavlink_log_critical(_mavlink_fd, "got drop position and approval");
break;
default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
break;
default:
src/modules/bottle_drop/module.mk
+4
View File
@@ -39,3 +39,7 @@ MODULE_COMMAND = bottle_drop
SRCS = bottle_drop.cpp \
bottle_drop_params.c
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1200
src/modules/commander/accelerometer_calibration.cpp
+32 -18
View File
@@ -83,7 +83,7 @@
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
@@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = {
0.0f,
1.0f,
@@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) {
res = ERROR;
break;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(3);
continue;
}
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
/* inform user about already handled side */
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(4);
continue;
}
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
@@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0;
}
}
src/modules/commander/airspeed_calibration.cpp
+17 -8
View File
@@ -61,6 +61,15 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
@@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
close(diff_pres_sub);
return ERROR;
}
@@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
src/modules/commander/commander.cpp
+249 -159
View File
@@ -207,7 +207,9 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub);
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
orb_advert_t *home_pub);
/**
* Mainloop of commander.
@@ -218,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@@ -230,7 +230,8 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
@@ -260,7 +261,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2950,
3200,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -393,7 +394,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -406,11 +408,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
}
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
&& (cmd->target_component != 0))) { // component_id 0: valid for all components
return false;
}
@@ -537,13 +540,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else {
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
(double)cmd->param4,
(double)cmd->param5,
(double)cmd->param6,
(double)cmd->param7);
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
(double)cmd->param4,
(double)cmd->param5,
(double)cmd->param6,
(double)cmd->param7);
}
}
break;
@@ -554,35 +557,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe (termination)");
} else {
armed_local->force_failsafe = false;
warnx("disabling failsafe (termination)");
}
/* param2 is currently used for other failsafe modes */
status_local->engine_failure_cmd = false;
status_local->data_link_lost_cmd = false;
status_local->gps_failure_cmd = false;
status_local->rc_signal_lost_cmd = false;
if ((int)cmd->param2 <= 0) {
/* reset all commanded failure modes */
warnx("reset all non-flighttermination failsafe commands");
} else if ((int)cmd->param2 == 1) {
/* trigger engine failure mode */
status_local->engine_failure_cmd = true;
warnx("engine failure mode commanded");
} else if ((int)cmd->param2 == 2) {
/* trigger data link loss mode */
status_local->data_link_lost_cmd = true;
warnx("data link loss mode commanded");
} else if ((int)cmd->param2 == 3) {
/* trigger gps loss mode */
status_local->gps_failure_cmd = true;
warnx("gps loss mode commanded");
} else if ((int)cmd->param2 == 4) {
/* trigger rc loss mode */
status_local->rc_signal_lost_cmd = true;
warnx("rc loss mode commanded");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
@@ -633,21 +644,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
}
break;
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
status_local->offboard_control_set_by_command = false;
} else {
/* Set flag that offboard was set via command, main state is not overridden by rc */
status_local->offboard_control_set_by_command = true;
}
} else {
/* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */
@@ -656,6 +673,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -740,7 +758,10 @@ int commander_thread_main(int argc, char *argv[])
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
@@ -802,6 +823,7 @@ int commander_thread_main(int argc, char *argv[])
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
@@ -827,11 +849,14 @@ int commander_thread_main(int argc, char *argv[])
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
orb_advert_t mission_pub = -1;
mission_s mission;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
mission.current_seq);
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq);
mission.dataman_id, mission.count, mission.current_seq);
} else {
const char *missionfail = "reading mission state failed";
warnx("%s", missionfail);
@@ -1097,6 +1122,7 @@ int commander_thread_main(int argc, char *argv[])
status.offboard_control_signal_lost = false;
status_changed = true;
}
} else {
if (!status.offboard_control_signal_lost) {
status.offboard_control_signal_lost = true;
@@ -1115,9 +1141,9 @@ int commander_thread_main(int argc, char *argv[])
/* perform system checks when new telemetry link connected */
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
(void)rc_calibration_check(mavlink_fd);
}
@@ -1130,6 +1156,7 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
/* Check if the barometer is healthy and issue a warning in the GCS if not so.
* Because the barometer is used for calculating AMSL altitude which is used to ensure
* vertical separation from other airtraffic the operator has to know when the
@@ -1142,6 +1169,7 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
mavlink_log_critical(mavlink_fd, "baro healthy");
}
} else {
if (!status.barometer_failure) {
status.barometer_failure = true;
@@ -1164,10 +1192,11 @@ int commander_thread_main(int argc, char *argv[])
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status.condition_power_input_valid = false;
} else {
status.condition_power_input_valid = true;
}
@@ -1187,9 +1216,11 @@ int commander_thread_main(int argc, char *argv[])
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1233,7 +1264,8 @@ int commander_thread_main(int argc, char *argv[])
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
&status_changed);
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
@@ -1283,8 +1315,11 @@ int commander_thread_main(int argc, char *argv[])
local_eph_good = false;
}
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
&& local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
&(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
@@ -1315,7 +1350,8 @@ int commander_thread_main(int argc, char *argv[])
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
throttle);
}
}
@@ -1372,8 +1408,8 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
//struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
struct stat statbuf;
on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
@@ -1383,26 +1419,30 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
}
status_changed = true;
}
@@ -1411,7 +1451,8 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1439,23 +1480,25 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize map projection if gps is valid */
if (!map_projection_global_initialized()
&& (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
&& (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) {
/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
}
/* check if GPS fix is ok */
if (status.circuit_breaker_engaged_gpsfailure_check ||
(gps_position.fix_type >= 3 &&
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
(gps_position.fix_type >= 3 &&
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
/* handle the case where gps was regained */
if (status.gps_failure) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps regained");
}
} else {
if (!status.gps_failure) {
status.gps_failure = true;
@@ -1477,12 +1520,14 @@ int commander_thread_main(int argc, char *argv[])
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
@@ -1490,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
/* RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@@ -1515,11 +1560,15 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
stick_off_counter = 0;
} else {
@@ -1541,8 +1590,11 @@ int commander_thread_main(int argc, char *argv[])
*/
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1565,6 +1617,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
mavlink_log_info(mavlink_fd, "DISARMED by RC");
}
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
@@ -1601,18 +1654,20 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
have_link = true;
} else if (!telemetry_lost[i]) {
/* telemetry was healthy also in last iteration
* we don't have to check a timeout */
@@ -1621,6 +1676,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true;
@@ -1648,24 +1704,26 @@ int commander_thread_main(int argc, char *argv[])
* only for fixed wing for now
*/
if (!status.circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
battery.current_a/actuator_controls.control[3] <
ef_current2throttle_thres) ||
(status.engine_failure))) {
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
battery.current_a / actuator_controls.control[3] <
ef_current2throttle_thres) ||
(status.engine_failure))) {
/* potential failure, measure time */
if (timestamp_engine_healthy > 0 &&
hrt_elapsed_time(&timestamp_engine_healthy) >
ef_time_thres * 1e6 &&
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "Engine Failure");
hrt_elapsed_time(&timestamp_engine_healthy) >
ef_time_thres * 1e6 &&
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "Engine Failure");
}
} else {
/* no failure reset flag */
timestamp_engine_healthy = hrt_absolute_time();
if (status.engine_failure) {
status.engine_failure = false;
status_changed = true;
@@ -1692,20 +1750,22 @@ int commander_thread_main(int argc, char *argv[])
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state != MAIN_STATE_MANUAL &&
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of data link loss && gps failure");
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
}
}
@@ -1714,19 +1774,21 @@ int commander_thread_main(int argc, char *argv[])
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state == MAIN_STATE_ACRO ||
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of RC signal loss && gps failure");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
}
}
@@ -1767,6 +1829,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
}
arming_state_changed = false;
}
@@ -1787,7 +1850,11 @@ int commander_thread_main(int argc, char *argv[])
if (status.failsafe != failsafe_old) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
if (status.failsafe) {
mavlink_log_critical(mavlink_fd, "failsafe mode on");
} else {
mavlink_log_critical(mavlink_fd, "failsafe mode off");
}
failsafe_old = status.failsafe;
}
@@ -1811,7 +1878,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available
&& safety.safety_off))) {
/* play tune when armed */
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
@@ -1989,6 +2057,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -2010,6 +2079,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} else {
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
@@ -2024,7 +2094,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "POSCTL");
}
// fallback to ALTCTL
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@@ -2048,14 +2118,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_RTL");
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
@@ -2064,7 +2134,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_LOITER");
print_reject_mode(status_local, "AUTO_LOITER");
} else {
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
@@ -2073,22 +2143,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_MISSION");
print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
}
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
@@ -2132,18 +2202,6 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@@ -2156,59 +2214,6 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
switch (sp_offboard.mode) {
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_force_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
//XXX: the flags could depend on sp_offboard.ignore
break;
default:
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
}
break;
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@@ -2224,6 +2229,7 @@ set_control_mode()
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RCRECOVER:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
@@ -2249,6 +2255,19 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
@@ -2262,6 +2281,19 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
@@ -2275,6 +2307,63 @@ set_control_mode()
control_mode.flag_control_termination_enabled = true;
break;
case NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
switch (sp_offboard.mode) {
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_force_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
//XXX: the flags could depend on sp_offboard.ignore
break;
default:
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
}
break;
default:
break;
}
@@ -2416,7 +2505,8 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
src/modules/commander/gyro_calibration.cpp
+4 -1
View File
@@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale = {
0.0f,
src/modules/commander/mag_calibration.cpp
+1 -1
View File
@@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0;
src/modules/commander/state_machine_helper.cpp
+19 -16
View File
@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
* - depending on datalink, RC and if the mission is finished */
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
/* Finished handling commands which have priority , now handle failures */
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
/* datalink loss disabled:
* check if both, RC and datalink are lost during the mission
* or RC is lost after the mission is finished: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost && !stay_in_failsafe) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
src/modules/controllib/module.mk
+2
View File
@@ -39,3 +39,5 @@ SRCS = test_params.c \
block/BlockParam.cpp \
uorb/blocks.cpp \
blocks.cpp
MAXOPTIMIZATION = -Os
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+6 -3
View File
@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
warn_index = max_warn_index;
}
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
// Do not warn about accel offset if we have no position updates
if (!(warn_index == 5 && _ekf->staticMode)) {
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
}
}
struct estimator_status_report rep;
@@ -1557,7 +1560,7 @@ FixedwingEstimator::start()
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
5000,
7500,
(main_t)&FixedwingEstimator::task_main_trampoline,
nullptr);
src/modules/mavlink/mavlink.c
+1 -5
View File
@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
50,
MAV_TYPE_FIXED_WING,
0,
0,
0
50
}; // System ID, 1-255, Component/Subsystem ID, 1-255
/*
src/modules/mavlink/mavlink_main.cpp
+9 -7
View File
@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
_system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
_system_type = system_type;
}
int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -763,7 +765,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2700,
2800,
(main_t)&Mavlink::start_helper,
(const char **)argv);
src/modules/mavlink/mavlink_main.h
+4
View File
@@ -265,6 +265,8 @@ public:
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
protected:
Mavlink *next;
@@ -354,6 +356,8 @@ private:
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
unsigned _system_type;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
src/modules/mavlink/mavlink_messages.cpp
+57 -9
View File
@@ -302,7 +302,7 @@ protected:
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
msg.type = mavlink_system.type;
msg.type = _mavlink->get_system_type();
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
@@ -382,11 +382,11 @@ protected:
clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
struct tm t;
gmtime_r(&gps_time_sec, &t);
struct tm tt;
gmtime_r(&gps_time_sec, &tt);
// XXX we do not want to interfere here with the SD log app
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
}
@@ -1353,15 +1353,17 @@ protected:
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (mavlink_system.type) {
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
@@ -1717,7 +1719,53 @@ protected:
msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
msg.rssi = rc.rssi;
/* RSSI has a max value of 100, and when Spektrum or S.BUS are
* available, the RSSI field is invalid, as they do not provide
* an RSSI measurement. Use an out of band magic value to signal
* these digital ports. XXX revise MAVLink spec to address this.
* One option would be to use the top bit to toggle between RSSI
* and input source mode.
*
* Full RSSI field: 0b 1 111 1111
*
* ^ If bit is set, RSSI encodes type + RSSI
*
* ^ These three bits encode a total of 8
* digital RC input types.
* 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
* ^ These four bits encode a total of
* 16 RSSI levels. 15 = full, 0 = no signal
*
*/
/* Initialize RSSI with the special mode level flag */
msg.rssi = (1 << 7);
/* Set RSSI */
msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
switch (rc.input_source) {
case RC_INPUT_SOURCE_PX4FMU_PPM:
/* fallthrough */
case RC_INPUT_SOURCE_PX4IO_PPM:
msg.rssi |= (0 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
msg.rssi |= (1 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_SBUS:
msg.rssi |= (2 << 4);
break;
case RC_INPUT_SOURCE_PX4IO_ST24:
msg.rssi |= (3 << 4);
break;
}
if (rc.rc_lost) {
/* RSSI is by definition zero */
msg.rssi = 0;
}
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
}
src/modules/mavlink/mavlink_receiver.cpp
+5 -7
View File
@@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.flow_raw_x = flow.integrated_x;
f.flow_raw_y = flow.integrated_y;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
@@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.ground_distance;
r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
@@ -1398,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
struct sched_param param;
(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
src/modules/navigator/datalinkloss.cpp
+4
View File
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
src/modules/navigator/mission.cpp
+17 -11
View File
@@ -371,7 +371,7 @@ Mission::set_mission_items()
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
return false;
}
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
* 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
return false;
}
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (is_current) {
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
&mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"ERROR DO JUMP waypoint could not be written");
"ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
if (is_current) {
mavlink_log_critical(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
}
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
reset_mission_item_reached();
}
void
src/modules/navigator/navigator_main.cpp
+4 -6
View File
@@ -289,8 +289,9 @@ Navigator::task_main()
navigation_capabilities_update();
params_update();
/* rate limit position updates to 50 Hz */
/* rate limit position and sensor updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
orb_set_interval(_sensor_combined_sub, 20);
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
@@ -497,8 +498,8 @@ Navigator::start()
/* start the task */
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
SCHED_PRIORITY_DEFAULT + 20,
1800,
(main_t)&Navigator::task_main_trampoline,
nullptr);
@@ -630,9 +631,6 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
}
void
src/modules/navigator/rcloss.cpp
+2
View File
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
}
break;
case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
src/modules/px4iofirmware/controls.c
+23 -2
View File
@@ -41,6 +41,7 @@
#include <stdbool.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
@@ -60,6 +61,8 @@ static perf_counter_t c_gather_ppm;
static int _dsm_fd;
static uint16_t rc_value_override = 0;
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
{
perf_begin(c_gather_dsm);
@@ -68,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -89,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
@@ -168,6 +171,12 @@ controls_tick() {
perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
}
if (st24_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@@ -316,6 +325,9 @@ controls_tick() {
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
/* pick out override channel, indicated by special mapping */
rc_value_override = SIGNED_TO_REG(scaled);
}
}
}
@@ -409,9 +421,18 @@ controls_tick() {
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
/*
if the FMU is dead then enable override if we have a
mixer and OVERRIDE_IMMEDIATE is set
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
override = true;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
src/modules/px4iofirmware/mixer.cpp
-7
View File
@@ -60,13 +60,6 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 500000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
#define OVERRIDE 4
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
static bool should_arm = false;
src/modules/px4iofirmware/protocol.h
+3
View File
@@ -183,6 +183,7 @@
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@@ -221,6 +222,7 @@ enum { /* DSM bind states */
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */
@@ -246,6 +248,7 @@ enum { /* DSM bind states */
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
src/modules/px4iofirmware/px4io.c
+67
View File
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
#define NUM_MSG 2
static char msg[NUM_MSG][40];
static void heartbeat_blink(void);
static void ring_blink(void);
/*
* add a debug message to be printed on the console
*/
@@ -126,6 +129,65 @@ heartbeat_blink(void)
LED_BLUE(heartbeat = !heartbeat);
}
static void
ring_blink(void)
{
#ifdef GPIO_LED4
if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
LED_RING(1);
return;
}
// XXX this led code does have
// intentionally a few magic numbers.
const unsigned max_brightness = 118;
static unsigned counter = 0;
static unsigned brightness = max_brightness;
static unsigned brightness_counter = 0;
static unsigned on_counter = 0;
if (brightness_counter < max_brightness) {
bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
// XXX once led is PWM driven,
// remove the ! in the line below
// to return to the proper breathe
// animation / pattern (currently inverted)
LED_RING(!on);
brightness_counter++;
if (on) {
on_counter++;
}
} else {
if (counter >= 62) {
counter = 0;
}
int n;
if (counter < 32) {
n = counter;
} else {
n = 62 - counter;
}
brightness = (n * n) / 8;
brightness_counter = 0;
on_counter = 0;
counter++;
}
#endif
}
static uint64_t reboot_time;
/**
@@ -191,6 +253,9 @@ user_start(int argc, char *argv[])
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
@@ -294,6 +359,8 @@ user_start(int argc, char *argv[])
heartbeat_blink();
}
ring_blink();
check_reboot();
/* check for debug activity (default: none) */
src/modules/px4iofirmware/px4io.h
+1
View File
@@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit;
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s))
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
src/modules/px4iofirmware/registers.c
+10 -2
View File
@@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -603,6 +604,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
case PX4IO_P_SETUP_FORCE_SAFETY_ON:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
break;
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
@@ -686,7 +693,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
src/modules/px4iofirmware/sbus.c
+70 -25
View File
@@ -57,11 +57,12 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
*/
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -87,13 +89,15 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
{
if (sbus_fd < 0)
if (sbus_fd < 0) {
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
}
if (sbus_fd >= 0) {
struct termios t;
@@ -113,16 +117,49 @@ sbus_init(const char *device)
} else {
debug("S.Bus: open failed");
}
return sbus_fd;
}
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
char a = 'A';
write(sbus_fd, &a, 1);
}
uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
uint8_t offset = 0;
uint16_t value;
hrt_abstime now;
now = hrt_absolute_time();
if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
last_txframe_time = now;
uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
/* 16 is sbus number of servos/channels minus 2 single bit channels.
* currently ignoring single bit channels. */
for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
/*protect from out of bounds values and limit to 11 bits*/
if (value > 0x07ff ) {
value = 0x07ff;
}
while (offset >= 8) {
++byteindex;
offset -= 8;
}
oframe[byteindex] |= (value << (offset)) & 0xff;
oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
offset += 11;
}
write(sbus_fd, oframe, SBUS_FRAME_SIZE);
}
}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
@@ -167,8 +204,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
if (ret < 1) {
return false;
}
last_rx_time = now;
@@ -180,8 +218,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
/*
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
if (partial_frame_count < SBUS_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -228,7 +267,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
@@ -237,23 +277,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
}
switch (frame[24]) {
case 0x00:
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
@@ -283,7 +327,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
@@ -304,16 +348,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true;
*sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
src/modules/sdlog2/module.mk
+2
View File
@@ -43,3 +43,5 @@ SRCS = sdlog2.c \
logbuffer.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/sdlog2/sdlog2.c
+33 -4
View File
@@ -90,6 +90,7 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/encoders.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -954,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct servorail_status_s servorail_status;
struct satellite_info_s sat_info;
struct wind_estimate_s wind_estimate;
struct encoders_s encoders;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -996,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GS1B_s log_GS1B;
struct log_TECS_s log_TECS;
struct log_WIND_s log_WIND;
struct log_ENCD_s log_ENCD;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1033,12 +1036,12 @@ int sdlog2_thread_main(int argc, char *argv[])
int system_power_sub;
int servorail_status_sub;
int wind_sub;
int encoders_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1057,9 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
@@ -1068,6 +1068,25 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
/* add new topics HERE */
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
if (_extended_logging) {
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
}
/* close non-needed fd's */
/* close stdin */
close(0);
/* close stdout */
close(1);
thread_running = true;
@@ -1652,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(WIND);
}
/* --- ENCODERS --- */
if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
log_msg.msg_type = LOG_ENCD_MSG;
log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1];
log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1];
LOGBUFFER_WRITE_AND_COUNT(ENCD);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+11
View File
@@ -407,6 +407,16 @@ struct log_VISN_s {
float qw;
};
/* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 39
struct log_ENCD_s {
int64_t cnt0;
float vel0;
int64_t cnt1;
float vel1;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
src/modules/sensors/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = sensors.cpp \
sensor_params.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/systemlib/mcu_version.c
+109
View File
@@ -0,0 +1,109 @@
/****************************************************************************
*
* Copyright (c) 2014 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mcu_version.c
*
* Read out the microcontroller version from the board
*
* @author Lorenz Meier <lorenz@px4.io>
*
*/
#include "mcu_version.h"
#include <nuttx/config.h>
#ifdef CONFIG_ARCH_CHIP_STM32
#include <up_arch.h>
#define DBGMCU_IDCODE 0xE0042000
#define STM32F40x_41x 0x413
#define STM32F42x_43x 0x419
#define REVID_MASK 0xFFFF0000
#define DEVID_MASK 0xFFF
#endif
int mcu_version(char* rev, char** revstr)
{
#ifdef CONFIG_ARCH_CHIP_STM32
uint32_t abc = getreg32(DBGMCU_IDCODE);
int32_t chip_version = abc & DEVID_MASK;
enum MCU_REV revid = (abc & REVID_MASK) >> 16;
switch (chip_version) {
case STM32F40x_41x:
*revstr = "STM32F40x";
break;
case STM32F42x_43x:
*revstr = "STM32F42x";
break;
default:
*revstr = "STM32F???";
break;
}
switch (revid) {
case MCU_REV_STM32F4_REV_A:
*rev = 'A';
break;
case MCU_REV_STM32F4_REV_Z:
*rev = 'Z';
break;
case MCU_REV_STM32F4_REV_Y:
*rev = 'Y';
break;
case MCU_REV_STM32F4_REV_1:
*rev = '1';
break;
case MCU_REV_STM32F4_REV_3:
*rev = '3';
break;
default:
*rev = '?';
revid = -1;
break;
}
return revid;
#else
return -1;
#endif
}
src/modules/systemlib/mcu_version.h
+52
View File
@@ -0,0 +1,52 @@
/****************************************************************************
*
* Copyright (c) 2014 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/* magic numbers from reference manual */
enum MCU_REV {
MCU_REV_STM32F4_REV_A = 0x1000,
MCU_REV_STM32F4_REV_Z = 0x1001,
MCU_REV_STM32F4_REV_Y = 0x1003,
MCU_REV_STM32F4_REV_1 = 0x1007,
MCU_REV_STM32F4_REV_3 = 0x2001
};
/**
* Reports the microcontroller version of the main CPU.
*
* @param rev The silicon revision character
* @param revstr The full chip name string
* @return The silicon revision / version number as integer
*/
__EXPORT int mcu_version(char* rev, char** revstr);
src/modules/systemlib/mixer/mixer_load.c
+2
View File
@@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
warnx("line too long");
fclose(fp);
return -1;
}
@@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
strcat(buf, line);
}
fclose(fp);
return 0;
}
src/modules/systemlib/module.mk
+3 -1
View File
@@ -53,5 +53,7 @@ SRCS = err.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c \
circuit_breaker.c
circuit_breaker.c \
mcu_version.c
MAXOPTIMIZATION = -Os
src/modules/uORB/module.mk
+2
View File
@@ -44,3 +44,5 @@ SRCS = uORB.cpp \
objects_common.cpp \
Publication.cpp \
Subscription.cpp
MAXOPTIMIZATION = -Os
src/modules/uORB/objects_common.cpp
+3
View File
@@ -201,6 +201,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
#include "topics/test_motor.h"
ORB_DEFINE(test_motor, struct test_motor_s);
#include "topics/debug_key_value.h"
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
src/modules/uORB/topics/manual_control_setpoint.h
+8 -8
View File
@@ -47,7 +47,7 @@
* Switch position
*/
typedef enum {
SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
}; /**< manual control inputs */
switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
};
/**
* @}
src/modules/uORB/topics/rc_channels.h
+7 -3
View File
@@ -34,6 +34,8 @@
/**
* @file rc_channels.h
* Definition of the rc_channels uORB topic.
*
* @deprecated DO NOT USE FOR NEW CODE
*/
#ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
AUX_2,
AUX_3,
AUX_4,
AUX_5,
RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
AUX_5
};
// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
#define RC_CHANNELS_FUNCTION_MAX 18
/**
* @addtogroup topics
* @{
@@ -76,7 +81,6 @@ struct rc_channels_s {
uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */
float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */
uint8_t channel_count; /**< Number of valid channels */
char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */
int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */
uint8_t rssi; /**< Receive signal strength index */
bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */
src/modules/uORB/topics/test_motor.h
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (C) 2014 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_motor.h
*
* Test a single drive motor while the vehicle is disarmed
*
* Publishing values to this topic causes a single motor to spin, e.g. for
* ground test purposes. This topic will be ignored while the vehicle is armed.
*
*/
#ifndef TOPIC_TEST_MOTOR_H
#define TOPIC_TEST_MOTOR_H
#include <stdint.h>
#include "../uORB.h"
#define NUM_MOTOR_OUTPUTS 8
/**
* @addtogroup topics
* @{
*/
struct test_motor_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
unsigned motor_number; /**< number of motor to spin */
float value; /**< output power, range [0..1] */
};
/**
* @}
*/
/* actuator output sets; this list can be expanded as more drivers emerge */
ORB_DECLARE(test_motor);
#endif
src/modules/uORB/topics/vehicle_status.h
+1 -1
View File
@@ -108,7 +108,7 @@ typedef enum {
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
src/modules/uavcan/actuators/esc.cpp
+22 -5
View File
@@ -40,6 +40,9 @@
#include "esc.hpp"
#include <systemlib/err.h>
#define MOTOR_BIT(x) (1<<(x))
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
@@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
if (_armed) {
for (unsigned i = 0; i < num_outputs; i++) {
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
@@ -111,6 +113,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
}
@@ -121,9 +125,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
void UavcanEscController::arm_esc(bool arm)
void UavcanEscController::arm_all_escs(bool arm)
{
_armed = arm;
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
}
void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
src/modules/uavcan/actuators/esc.hpp
+7 -1
View File
@@ -61,7 +61,8 @@ public:
void update_outputs(float *outputs, unsigned num_outputs);
void arm_esc(bool arm);
void arm_all_escs(bool arm);
void arm_single_esc(int num, bool arm);
private:
/**
@@ -98,6 +99,11 @@ private:
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
uint32_t _armed_mask = 0;
/*
* Perf counters
*/
src/modules/uavcan/sensors/mag.cpp
+2
View File
@@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
src/modules/uavcan/sensors/sensor_bridge.cpp
+3
View File
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
return;
}
// update device id as we now know our device node_id
_device_id.devid_s.address = static_cast<uint8_t>(node_id);
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
src/modules/uavcan/sensors/sensor_bridge.hpp
+2
View File
@@ -112,6 +112,8 @@ protected:
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;
}
/**
src/modules/uavcan/uavcan_main.cpp
+24 -5
View File
@@ -279,6 +279,7 @@ int UavcanNode::run()
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,13 @@ int UavcanNode::run()
}
// can we mix?
if (controls_updated && (_mixers != nullptr)) {
if (_test_in_progress) {
float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
// Output to the bus
_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
} else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
@@ -384,15 +391,27 @@ int UavcanNode::run()
}
}
// Check arming state
// Check motor test state
bool updated = false;
orb_check(_test_motor_sub, &updated);
if (updated) {
orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
// Update the test status and check that we're not locked down
_test_in_progress = (_test_motor.value > 0);
_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
}
// Check arming state
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
// Update the armed status and check that we're not locked down
bool set_armed = _armed.armed && !_armed.lockdown;
// Update the armed status and check that we're not locked down and motor
// test is not running
bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
arm_actuators(set_armed);
}
@@ -429,7 +448,7 @@ int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
_esc_controller.arm_esc(arm);
_esc_controller.arm_all_escs(arm);
return OK;
}
src/modules/uavcan/uavcan_main.hpp
+5
View File
@@ -41,6 +41,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/test_motor.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
@@ -103,6 +104,10 @@ private:
actuator_armed_s _armed; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
test_motor_s _test_motor;
bool _test_in_progress = false;
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
src/systemcmds/motor_test/module.mk
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2014 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build the motor_test tool.
#
MODULE_COMMAND = motor_test
SRCS = motor_test.c
MODULE_STACKSIZE = 4096
MAXOPTIMIZATION = -Os
src/systemcmds/motor_test/motor_test.c
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: Holger Steinhaus <hsteinhaus@gmx.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file motor_test.c
*
* Tool for drive testing
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/test_motor.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
__EXPORT int motor_test_main(int argc, char *argv[]);
static void motor_test(unsigned channel, float value);
static void usage(const char *reason);
void motor_test(unsigned channel, float value)
{
orb_advert_t _test_motor_pub;
struct test_motor_s _test_motor;
_test_motor.motor_number = channel;
_test_motor.timestamp = hrt_absolute_time();
_test_motor.value = value;
if (_test_motor_pub > 0) {
/* publish test state */
orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
} else {
/* advertise and publish */
_test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
}
}
static void usage(const char *reason)
{
if (reason != NULL) {
warnx("%s", reason);
}
errx(1,
"usage:\n"
"motor_test\n"
" -m <channel> Motor to test (0..7)\n"
" -p <power> Power (0..100)\n");
}
int motor_test_main(int argc, char *argv[])
{
unsigned long channel = 0;
unsigned long lval;
float value = 0.0f;
int ch;
if (argc != 5) {
usage("please specify motor and power");
}
while ((ch = getopt(argc, argv, "m:p:")) != EOF) {
switch (ch) {
case 'm':
/* Read in motor number */
channel = strtoul(optarg, NULL, 0);
break;
case 'p':
/* Read in power value */
lval = strtoul(optarg, NULL, 0);
if (lval > 100)
usage("value invalid");
value = ((float)lval)/100.f;
break;
default:
usage(NULL);
}
}
motor_test(channel, value);
printf("motor %d set to %.2f\n", channel, (double)value);
exit(0);
}
src/systemcmds/nshterm/module.mk
+3 -1
View File
@@ -38,6 +38,8 @@
MODULE_COMMAND = nshterm
SRCS = nshterm.c
MODULE_STACKSIZE = 1400
MODULE_STACKSIZE = 1600
MAXOPTIMIZATION = -Os
MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30"
src/systemcmds/nshterm/nshterm.c
+27 -4
View File
@@ -51,6 +51,8 @@
#include <fcntl.h>
#include <systemlib/err.h>
#include <uORB/topics/actuator_armed.h>
__EXPORT int nshterm_main(int argc, char *argv[]);
int
@@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[])
}
unsigned retries = 0;
int fd = -1;
int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
struct actuator_armed_s armed;
/* we assume the system does not provide arming status feedback */
bool armed_updated = false;
/* try the first 30 seconds or if arming system is ready */
while ((retries < 300) || armed_updated) {
/* abort if an arming topic is published and system is armed */
bool updated = false;
if (orb_check(armed_fd, &updated)) {
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
armed_updated = true;
orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
if (armed.armed) {
/* this is not an error, but we are done */
exit(0);
}
}
/* try the first 30 seconds */
while (retries < 300) {
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
@@ -87,7 +110,7 @@ nshterm_main(int argc, char *argv[])
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
warnx("ERROR get termios config %s: %d\n", argv[1], termios_state);
warnx("ERR get config %s: %d\n", argv[1], termios_state);
close(fd);
return -1;
}
@@ -96,7 +119,7 @@ nshterm_main(int argc, char *argv[])
uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/);
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]);
warnx("ERR set config %s\n", argv[1]);
close(fd);
return -1;
}
src/systemcmds/pwm/pwm.c
+34 -34
View File
@@ -74,34 +74,34 @@ usage(const char *reason)
"usage:\n"
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n"
" arm Arm output\n"
" disarm Disarm output\n"
"arm\t\t\t\tArm output\n"
"disarm\t\t\t\tDisarm output\n"
"\n"
" rate ... Configure PWM rates\n"
" [-g <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"rate ...\t\t\tConfigure PWM rates\n"
"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n"
"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
"\n"
" failsafe ... Configure failsafe PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"failsafe ...\t\t\tFailsafe PWM\n"
"disarmed ...\t\t\tDisarmed PWM\n"
"min ...\t\t\t\tMinimum PWM\n"
"max ...\t\t\t\tMaximum PWM\n"
"\t[-c <channels>]\t\t(e.g. 1234)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n"
"\t-p <pwm value>\t\tPWM value\n"
"\n"
" test ... Directly set PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"test ...\t\t\tDirectly set PWM\n"
"\t[-c <channels>]\t\t(e.g. 1234)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n"
"\t-p <pwm value>\t\tPWM value\n"
"\n"
" info Print information about the PWM device\n"
"info\t\t\t\tPrint information\n"
"\n"
" -v Print verbose information\n"
" -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
"\t-v\t\t\tVerbose\n"
"\t-d <dev>\t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n"
);
}
@@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[])
unsigned single_ch = 0;
unsigned pwm_value = 0;
if (argc < 1)
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
@@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[])
/* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
usage("BAD set_mask VAL");
break;
case 'a':
@@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[])
case 'p':
pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad PWM value provided");
usage("BAD PWM VAL");
break;
case 'r':
alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alternative rate provided");
usage("BAD rate VAL");
break;
default:
break;
@@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[])
}
if (print_verbose && set_mask > 0) {
warnx("Chose channels: ");
warnx("Channels: ");
printf(" ");
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
if (set_mask & 1<<i)
@@ -364,7 +364,7 @@ pwm_main(int argc, char *argv[])
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
usage("no PWM provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
@@ -383,7 +383,7 @@ pwm_main(int argc, char *argv[])
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting failsafe values");
errx(ret, "BAD input VAL");
}
exit(0);
@@ -393,7 +393,7 @@ pwm_main(int argc, char *argv[])
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
usage("no PWM provided");
/* get current servo values */
struct pwm_output_values last_spos;
@@ -487,7 +487,7 @@ pwm_main(int argc, char *argv[])
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
steps_timing_index++ ) {
warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
@@ -526,7 +526,7 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_SET(%d)", i);
}
}
warnx("Key pressed, user abort\n");
warnx("User abort\n");
exit(0);
}
}
@@ -675,7 +675,7 @@ pwm_main(int argc, char *argv[])
exit(0);
}
usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
usage(NULL);
return 0;
}
src/systemcmds/tests/tests_main.c
+2
View File
@@ -110,7 +110,9 @@ const struct {
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
{"mtd", test_mtd, 0},
#ifndef TESTS_MATHLIB_DISABLE
{"mathlib", test_mathlib, 0},
#endif
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
{NULL, NULL, 0}
};
src/systemcmds/ver/ver.c
+59 -25
View File
@@ -44,14 +44,16 @@
#include <string.h>
#include <version/version.h>
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
// string constants for version commands
/* string constants for version commands */
static const char sz_ver_hw_str[] = "hw";
static const char sz_ver_hwcmp_str[] = "hwcmp";
static const char sz_ver_git_str[] = "git";
static const char sz_ver_bdate_str[] = "bdate";
static const char sz_ver_gcc_str[] = "gcc";
static const char sz_ver_all_str[] = "all";
static const char mcu_ver_str[] = "mcu";
static void usage(const char *reason)
{
@@ -59,55 +61,87 @@ static void usage(const char *reason)
printf("%s\n", reason);
}
printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
}
__EXPORT int ver_main(int argc, char *argv[]);
int ver_main(int argc, char *argv[])
{
int ret = 1; //defaults to an error
/* defaults to an error */
int ret = 1;
// first check if there are at least 2 params
/* first check if there are at least 2 params */
if (argc >= 2) {
if (argv[1] != NULL) {
if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
printf("%s\n", HW_ARCH);
ret = 0;
} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
if (argc >= 3 && argv[2] != NULL) {
// compare 3rd parameter with HW_ARCH string, in case of match, return 0
/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
if (ret == 0) {
printf("ver hwcmp match: %s\n", HW_ARCH);
}
return ret;
} else {
errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
}
}
} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
printf("%s\n", FW_GIT);
ret = 0;
/* check if we want to show all */
bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
printf("%s %s\n", __DATE__, __TIME__);
ret = 0;
} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
printf("%s\n", __VERSION__);
ret = 0;
} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
printf("HW arch: %s\n", HW_ARCH);
printf("Build datetime: %s %s\n", __DATE__, __TIME__);
printf("FW git-hash: %s\n", FW_GIT);
printf("GCC toolchain: %s\n", __VERSION__);
ret = 0;
} else {
}
if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
printf("FW git-hash: %s\n", FW_GIT);
ret = 0;
}
if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
printf("Build datetime: %s %s\n", __DATE__, __TIME__);
ret = 0;
}
if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
printf("Toolchain: %s\n", __VERSION__);
ret = 0;
}
if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
char rev;
char* revstr;
int chip_version = mcu_version(&rev, &revstr);
if (chip_version < 0) {
printf("UNKNOWN MCU\n");
} else {
printf("MCU: %s, rev. %c\n", revstr, rev);
if (chip_version < MCU_REV_STM32F4_REV_3) {
printf("\nWARNING WARNING WARNING!\n"
"Revision %c has a silicon errata\n"
"This device can only utilize a maximum of 1MB flash safely!\n"
"http://px4.io/help/errata\n\n", rev);
}
}
ret = 0;
}
if (ret == 1) {
errx(1, "unknown command.\n");
}