dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into uavcan

This commit is contained in:
Pavel Kirienko 2014-07-12 17:38:49 +04:00
parent e64a28e736 66e840ebd7
commit 9a56892c2b
15 changed files with 217 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/3033_wingwing
View File

@ -32,7 +32,7 @@ then
param set FW_THR_CRUISE 0.65
fi
set MIXER FMU_Q
set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_DISARMED 1000

51
ROMFS/px4fmu_common/mixers/wingwing.mix Normal file
View File

@ -0,0 +1,51 @@
Delta-wing mixer for PX4FMU
===========================
Designed for Wing Wing Z-84
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 6500 6500 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 -6500 -6500 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000

26
Tools/check_submodules.sh
View File

@ -2,12 +2,19 @@
if [ -d NuttX/nuttx ];
then
STATUSRETVAL=$(git status --porcelain | grep -i "NuttX")
if [ "$STATUSRETVAL" == "" ]; then
STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked NuttX submodule, correct version found"
else
echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'"
echo ""
echo ""
echo "NuttX sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else
@ -18,12 +25,19 @@ fi
if [ -d mavlink/include/mavlink/v1.0 ];
then
STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0")
if [ "$STATUSRETVAL" == "" ]; then
STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked mavlink submodule, correct version found"
else
echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'"
echo ""
echo ""
echo "mavlink sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else

68
src/drivers/device/spi.cpp
View File

@ -133,26 +133,44 @@ SPI::probe()
int
SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
irqstate_t state;
int result;
if ((send == nullptr) && (recv == nullptr))
return -EINVAL;
/* lock the bus as required */
if (!up_interrupt_context()) {
switch (locking_mode) {
default:
case LOCK_PREEMPTION:
state = irqsave();
break;
case LOCK_THREADS:
SPI_LOCK(_dev, true);
break;
case LOCK_NONE:
break;
}
}
LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode;
/* lock the bus as required */
switch (mode) {
default:
case LOCK_PREEMPTION:
{
irqstate_t state = irqsave();
result = _transfer(send, recv, len);
irqrestore(state);
}
break;
case LOCK_THREADS:
SPI_LOCK(_dev, true);
result = _transfer(send, recv, len);
SPI_LOCK(_dev, false);
break;
case LOCK_NONE:
result = _transfer(send, recv, len);
break;
}
return result;
}
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
}
int
SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
@ -164,27 +182,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
if (!up_interrupt_context()) {
switch (locking_mode) {
default:
case LOCK_PREEMPTION:
irqrestore(state);
break;
case LOCK_THREADS:
SPI_LOCK(_dev, false);
break;
case LOCK_NONE:
break;
}
}
return OK;
}
void
SPI::set_frequency(uint32_t frequency)
{
_frequency = frequency;
}
} // namespace device

2
src/drivers/device/spi.h
View File

@ -129,6 +129,8 @@ private:
enum spi_mode_e _mode;
uint32_t _frequency;
struct spi_dev_s *_dev;
int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
};
} // namespace device

16
src/drivers/drv_px4flow.h
View File

@ -46,10 +46,18 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* @addtogroup topics
* @{
*/
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*
* @warning If possible the usage of the raw flow and performing rotation-compensation
* using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
@ -57,14 +65,18 @@ struct px4flow_report {
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/**
* @}
*/
/*
* ObjDev tag for px4flow data.
*/

9
src/drivers/drv_range_finder.h
View File

@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE {
RANGE_FINDER_TYPE_LASER = 0,
};
/**
* @addtogroup topics
* @{
*/
/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
@ -64,6 +69,10 @@ struct range_finder_report {
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
};
/**
* @}
*/
/*
* ObjDev tag for raw range finder data.
*/

9
src/drivers/drv_rc_input.h
View File

@ -67,6 +67,11 @@
*/
#define RC_INPUT_RSSI_MAX 255
/**
* @addtogroup topics
* @{
*/
/**
* Input signal type, value is a control position from zero to 100
* percent.
@ -141,6 +146,10 @@ struct rc_input_values {
rc_input_t values[RC_INPUT_MAX_CHANNELS];
};
/**
* @}
*/
/*
* ObjDev tag for R/C inputs.
*/

4
src/drivers/gps/gps.cpp
View File

@ -127,7 +127,7 @@ private:
/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
void config();
void config();
/**
* Trampoline to the worker task
@ -486,6 +486,8 @@ GPS::print_info()
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());

2
src/drivers/px4fmu/fmu.cpp
View File

@ -329,7 +329,7 @@ PX4FMU::init()
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
1600,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);

22
src/drivers/px4io/px4io_uploader.cpp
View File

@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
@ -413,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n)
int
PX4IO_Uploader::program(size_t fw_size)
{
uint8_t file_buf[PROG_MULTI_MAX];
uint8_t *file_buf;
ssize_t count;
int ret;
size_t sent = 0;
file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
if (!file_buf) {
log("Can't allocate program buffer");
return -ENOMEM;
}
log("programming %u bytes...", (unsigned)fw_size);
ret = lseek(_fw_fd, 0, SEEK_SET);
@ -425,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size)
while (sent < fw_size) {
/* get more bytes to program */
size_t n = fw_size - sent;
if (n > sizeof(file_buf)) {
n = sizeof(file_buf);
if (n > PROG_MULTI_MAX) {
n = PROG_MULTI_MAX;
}
count = read_with_retry(_fw_fd, file_buf, n);
@ -438,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size)
(int)errno);
}
if (count == 0)
if (count == 0) {
free(file_buf);
return OK;
}
sent += count;
@ -455,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size)
ret = get_sync(1000);
if (ret != OK)
if (ret != OK) {
free(file_buf);
return ret;
}
}
free(file_buf);
return OK;
}

24
src/modules/commander/commander.cpp
View File

@ -286,15 +286,22 @@ int commander_main(int argc, char *argv[])
exit(0);
}
/* commands needing the app to run below */
if (!thread_running) {
warnx("\tcommander not started");
exit(1);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\tcommander is running");
print_status();
} else {
warnx("\tcommander not started");
}
print_status();
exit(0);
}
if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
int checkres = prearm_check(&status, mavlink_fd_local);
close(mavlink_fd_local);
warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
exit(0);
}
@ -303,7 +310,7 @@ int commander_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "2")) {
if (!strcmp(argv[1], "disarm")) {
arm_disarm(false, mavlink_fd, "command line");
exit(0);
}
@ -324,6 +331,7 @@ void usage(const char *reason)
void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
/* read all relevant states */

35
src/modules/commander/state_machine_helper.cpp
View File

@ -70,8 +70,6 @@
#endif
static const int ERROR = -1;
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
bool failed = false;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
ret = fd;
failed = true;
goto system_eval;
}
@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
failed = true;
goto system_eval;
}
@ -644,33 +644,31 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_scale < 9.78f || accel_scale > 9.83f) {
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
}
if (accel_scale > 30.0f /* m/s^2 */) {
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
} else {
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
}
if (!status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
if (fd <= 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
ret = fd;
failed = true;
goto system_eval;
}
@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
}
}
system_eval:
close(fd);
return ret;
return (failed);
}

2
src/modules/commander/state_machine_helper.h
View File

@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */

55
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1051,10 +1051,16 @@ protected:
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
/* 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) {
/* set number of valid outputs depending on vehicle type */
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (mavlink_system.type) {
@ -1071,59 +1077,44 @@ protected:
break;
}
/* scale / assign outputs depending on system type */
float out[8];
for (unsigned i = 0; i < 8; i++) {
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
out[i] = 0.0f;
/* scale PWM out 900..2100 us to -1..1 for other channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
}
} else {
out[i] = -1.0f;
/* send 0 when disarmed */
out[i] = 0.0f;
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
} else {
/* fixed wing: scale all channels except throttle -1 .. 1
* because we know that we set the mixers up this way
*/
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
/* scale PWM out 900..2100 us to -1..1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
/* scale PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
}
};