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

merged release_v1.0.0 into master

This commit is contained in:
Lorenz Meier 2015-06-30 15:30:45 +02:00
parent 454becdae5 3ef6212155
commit 50ba1f7365
23 changed files with 1085 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -17,9 +17,9 @@ then
param set FW_T_TIME_CONST 5
param set PE_VELNE_NOISE 0.3
param set PE_VELD_NOISE 0.5
param set PE_VELD_NOISE 0.35
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 0.5
param set PE_POSD_NOISE 1.0
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0002
fi

4
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@ -5,9 +5,9 @@ set VEHICLE_TYPE mc
if [ $AUTOCNF == yes ]
then
param set PE_VELNE_NOISE 0.5
param set PE_VELD_NOISE 0.7
param set PE_VELD_NOISE 0.35
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
param set PE_POSD_NOISE 1.25
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0001

4
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
View File

@ -34,9 +34,9 @@ then
param set FW_RR_P 0.02
param set PE_VELNE_NOISE 0.5
param set PE_VELD_NOISE 0.7
param set PE_VELD_NOISE 0.3
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
param set PE_POSD_NOISE 1.25
param set PE_GBIAS_PNOISE 0.000001
param set PE_ABIAS_PNOISE 0.0001
fi

8
ROMFS/px4fmu_common/init.d/rcS
View File

@ -457,13 +457,13 @@ then
if [ $TTYS1_BUSY == yes ]
then
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
set MAVLINK_F "-r 1000 -d /dev/ttyS0"
set MAVLINK_F "-r 5000 -d /dev/ttyS0"
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Start MAVLink on default port: ttyS1
set MAVLINK_F "-r 1000"
set MAVLINK_F "-r 5000"
fi
fi
@ -479,11 +479,11 @@ then
# but this works for now
if param compare SYS_COMPANION 921600
then
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x
fi
if param compare SYS_COMPANION 57600
then
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x
fi
if param compare SYS_COMPANION 157600
then

24
ROMFS/px4fmu_test/mixers/IO_pass.mix Normal file
View File

@ -0,0 +1,24 @@
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000

2
makefiles/nuttx/upload.mk
View File

@ -15,7 +15,7 @@ ifeq ($(SYSTYPE),Darwin)
SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
endif
ifeq ($(SYSTYPE),Linux)
SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*"
SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*"
endif
ifeq ($(SERIAL_PORTS),)
SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"

10
src/drivers/l3gd20/l3gd20.cpp
View File

@ -179,6 +179,8 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
@ -1102,18 +1104,18 @@ L3GD20::test_error()
int
L3GD20::self_test()
{
/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
/* evaluate gyro offsets, complain if offset -> zero or larger than 25 dps */
if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
return 1;
if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
return 1;
if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f)
return 1;
if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
return 1;

7
src/modules/commander/commander_params.c
View File

@ -150,11 +150,12 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/**
* Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current/throttle value
* Engine failure triggers only below this current value
*
* @group Commander
* @min 0.0
* @max 7.0
* @max 30.0
* @unit ampere
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
@ -167,7 +168,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
* @group Commander
* @unit second
* @min 0.0
* @max 7.0
* @max 60.0
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);

20
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
View File

@ -141,13 +141,13 @@ PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
* Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7
*
* @min 0.05
* @max 5.0
* @min 0.2
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f);
/**
* Position noise in north-east (horizontal) direction
@ -163,21 +163,21 @@ PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
* Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.1
* @max 10.0
* @min 0.5
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.1
* @max 10.0
* @min 0.01
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);

4
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1762,11 +1762,11 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else {
} else if (_attitude_sp_pub <= 0 && !_vehicle_status.is_rotary_wing) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}

35
src/modules/mavlink/mavlink.c
View File

@ -33,9 +33,9 @@
/**
* @file mavlink.c
* Adapter functions expected by the protocol library
* Define MAVLink specific parameters
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_config.h>
@ -46,7 +46,6 @@
#include "mavlink_bridge_header.h"
#include <systemlib/param/param.h>
/* define MAVLink specific parameters */
/**
* MAVLink system ID
* @group MAVLink
@ -59,20 +58,36 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
* MAVLink component ID
* @group MAVLink
* @min 1
* @max 50
* @max 250
*/
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
/**
* MAVLink type
* MAVLink Radio ID
*
* When non-zero the MAVLink app will attempt to configure the
* radio to this ID and re-set the parameter to 0. If the value
* is negative it will reset the complete radio config to
* factory defaults.
*
* @group MAVLink
* @min -1
* @max 240
*/
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
/**
* Use/Accept HIL GPS message (even if not in HIL mode)
* MAVLink airframe type
*
* If set to 1 incomming HIL GPS messages are parsed.
* @min 1
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_TYPE, 1);
/**
* Use/Accept HIL GPS message even if not in HIL mode
*
* If set to 1 incoming HIL GPS messages are parsed.
*
* @group MAVLink
*/
@ -81,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
*
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
* control mode
* If set to 1 incoming external setpoint messages will be directly forwarded
* to the controllers if in offboard control mode
*
* @group MAVLink
*/

48
src/modules/mavlink/mavlink_main.cpp
View File

@ -145,6 +145,7 @@ Mavlink::Mavlink() :
_mavlink_ftp(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_radio_id(0),
_logbuffer {},
_total_counter(0),
_receive_thread {},
@ -184,6 +185,7 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_radio_id(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
@ -524,6 +526,7 @@ void Mavlink::mavlink_update_system(void)
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
_param_radio_id = param_find("MAV_RADIO_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
@ -539,6 +542,7 @@ void Mavlink::mavlink_update_system(void)
int32_t component_id;
param_get(_param_component_id, &component_id);
param_get(_param_radio_id, &_radio_id);
/* only allow system ID and component ID updates
* after reboot - not during operation */
@ -1662,6 +1666,50 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
}
/* radio config check */
if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
if (fs) {
/* switch to AT command mode */
usleep(1200000);
fprintf(fs, "+++\n");
usleep(1200000);
if (_radio_id > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);
} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
usleep(200000);
}
/* write config */
fprintf(fs, "AT&W");
usleep(200000);
/* reboot */
fprintf(fs, "ATZ");
usleep(200000);
// XXX NuttX suffers from a bug where
// fclose() also closes the fd, not just
// the file stream. Since this is a one-time
// config thing, we leave the file struct
// allocated.
//fclose(fs);
} else {
warnx("open fd %d failed", _uart_fd);
}
/* reset param and save */
_radio_id = 0;
param_set(_param_radio_id, &_radio_id);
}
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);

2
src/modules/mavlink/mavlink_main.h
View File

@ -355,6 +355,7 @@ private:
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
int32_t _radio_id;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
@ -424,6 +425,7 @@ private:
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_radio_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;

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

@ -1776,6 +1776,10 @@ protected:
msg.x = pos_sp.x;
msg.y = pos_sp.y;
msg.z = pos_sp.z;
msg.yaw = pos_sp.yaw;
msg.vx = pos_sp.vx;
msg.vy = pos_sp.vy;
msg.vz = pos_sp.vz;
_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
}

41
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -122,6 +122,7 @@ private:
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
@ -137,6 +138,7 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
@ -319,6 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_reset_alt_sp(true),
_mode_auto(false)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
@ -474,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
if (updated) {
@ -903,6 +912,7 @@ MulticopterPositionControl::task_main()
/*
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@ -1373,44 +1383,44 @@ MulticopterPositionControl::task_main()
reset_int_xy = true;
}
// generate attitude setpoint from manual controls
/* generate attitude setpoint from manual controls */
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
// reset yaw setpoint to current position if needed
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
}
// do not move yaw while arming
/* do not move yaw while arming */
else if (_manual.z > 0.1f)
{
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
// If the yaw offset became too big for the system to track stop
// shifting it
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}
}
//Control roll and pitch directly if we no aiding velocity controller is active
/* control roll and pitch directly if we no aiding velocity controller is active */
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
}
//Construct attitude setpoint rotation matrix
/* construct attitude setpoint rotation matrix */
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
@ -1435,10 +1445,9 @@ MulticopterPositionControl::task_main()
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr) {
if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
} else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}

17
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013-2015 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
@ -36,7 +35,7 @@
* @file mc_pos_control_params.c
* Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
@ -107,7 +106,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
*
* @unit m/s
* @min 0.0
* @max 8 m/s
* @max 8.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
@ -184,7 +183,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
*
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@ -196,7 +195,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
*
* Limits maximum tilt angle on landing.
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@ -215,7 +214,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
/**
* Max manual roll
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@ -225,7 +224,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@ -235,7 +234,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
*
* @unit deg/s
* @unit degree / s
* @min 0.0
* @group Multicopter Position Control
*/

15
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
_vel_sp(1)*_vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
}
if (!_control_mode->data().flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;

15
src/modules/navigator/datalinkloss_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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
@ -36,7 +36,7 @@
*
* Parameters for DLL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
@ -64,7 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
* Latitude of comms hold waypoint
*
* @unit degrees * 1e7
* @min 0
* @min -900000000
* @max 900000000
* @group Data Link Loss
*/
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
@ -75,7 +76,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
* Longitude of comms hold waypoint
*
* @unit degrees * 1e7
* @min 0
* @min -1800000000
* @max 1800000000
* @group Data Link Loss
*/
PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
@ -86,13 +88,14 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
* Altitude of comms hold waypoint
*
* @unit m
* @min 0.0
* @min -50
* @max 30000
* @group Data Link Loss
*/
PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
/**
* Aifield hole wait time
* Airfield hole wait time
*
* The amount of time in seconds the system should wait at the airfield home waypoint
*

26
src/modules/navigator/navigator_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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
@ -36,8 +36,8 @@
*
* Parameters for navigator in general
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
@ -49,9 +49,9 @@
*
* Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
*
* @unit meters
* @min 20
* @max 200
* @unit meter
* @min 25
* @max 1000
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
*
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @unit meter
* @min 0.05
* @max 200
* @max 200.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
@ -74,6 +74,7 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
@ -84,6 +85,7 @@ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
@ -94,7 +96,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
* Latitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0
* @min -900000000
* @max 900000000
* @group Data Link Loss
*/
PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
@ -105,7 +108,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
* Longitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0
* @min -1800000000
* @max 1800000000
* @group Data Link Loss
*/
PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
@ -116,7 +120,7 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
* Altitude of airfield home waypoint
*
* @unit m
* @min 0.0
* @min -50
* @group Data Link Loss
*/
PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);

7
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -145,10 +145,9 @@ int position_estimator_inav_main(int argc, char *argv[])
inav_verbose_mode = false;
if (argc > 1)
if (!strcmp(argv[2], "-v")) {
inav_verbose_mode = true;
}
if (argc > 2 && !strcmp(argv[2], "-v")) {
inav_verbose_mode = true;
}
thread_should_exit = false;
position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",

863
src/modules/sensors/sensor_params.c
View File

File diff suppressed because it is too large Load Diff

26
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -43,10 +43,10 @@
/**
* VTOL number of engines
*
* @min 1
* @min 0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
PARAM_DEFINE_INT32(VT_MOT_COUNT, 0);
/**
* Idle speed of VTOL when in multicopter mode
@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
* @min 900
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
/**
* Minimum airspeed in multicopter mode
@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f);
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN, 10.0f);
/**
* Maximum airspeed in multicopter mode
@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f);
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX, 30.0f);
/**
* Trim airspeed when in multicopter mode
@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM, 10.0f);
/**
* Permanent stabilization in fw mode
@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
/**
* Fixed wing pitch trim
@ -107,7 +107,7 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f);
PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM, 0.0f);
/**
* Motor max power
@ -118,18 +118,18 @@ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f);
* @min 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f);
PARAM_DEFINE_FLOAT(VT_POWER_MAX, 120.0f);
/**
* Propeller efficiency parameter
*
* Influences propeller efficiency at different power settings. Should be tuned beforehand.
*
* @min 0.5
* @min 0.0
* @max 0.9
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
/**
* Total airspeed estimate low-pass filter gain
@ -140,7 +140,7 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
* @max 0.99
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f);
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1)
@ -160,4 +160,4 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0);
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0);

4
unittests/param_test.cpp
View File

@ -52,8 +52,8 @@ void _assert_parameter_int_value(param_t param, int32_t expected)
{
int32_t value;
int result = param_get(param, &value);
ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param);
ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param);
ASSERT_EQ(0, result) << printf("param_get (%lu) did not return parameter\n", param);
ASSERT_EQ(expected, value) << printf("value for param (%lu) doesn't match default value\n", param);
}
void _set_all_int_parameters_to(int32_t value)