fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 08:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Thomas Gubler
2014-05-13 21:25:08 +02:00
parent f20a9c4873 178a3e8567
commit 5aea635a89
128 changed files with 1381 additions and 7182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -38,3 +38,4 @@ tags
.tags_sorted_by_file
.pydevproject
.ropeproject
*.orig
Documentation/rc_mode_switch.odg
BIN
View File
Binary file not shown.
Documentation/rc_mode_switch.pdf
BIN
View File
Binary file not shown.
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+35
View File
@@ -0,0 +1,35 @@
#!nsh
#
# Steadidrone QU4D
#
# Thomas Gubler <thomasgubler@gmail.com>
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 0.5
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set BAT_N_CELLS 4
fi
set MIXER FMU_quad_w
set PWM_MIN 1210
set PWM_MAX 2100
set PWM_OUTPUTS 1234
ROMFS/px4fmu_common/init.d/rc.autostart
+5
View File
@@ -195,6 +195,11 @@ then
sh /etc/init.d/10016_3dr_iris
fi
if param compare SYS_AUTOSTART 10017
then
sh /etc/init.d/10017_steadidrone_qu4d
fi
#
# Hexa Coaxial
#
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+2 -2
View File
@@ -32,9 +32,9 @@ then
param set MPC_Z_VEL_D 0.0
param set MPC_Z_VEL_MAX 3
param set MPC_Z_FF 0.5
param set MPC_TILT_MAX 1.0
param set MPC_TILTMAX_AIR 45.0
param set MPC_TILTMAX_LND 15.0
param set MPC_LAND_SPEED 1.0
param set MPC_LAND_TILT 0.3
fi
set PWM_RATE 400
ROMFS/px4fmu_common/init.d/rcS
+10 -10
View File
@@ -419,16 +419,6 @@ then
mavlink start $MAVLINK_FLAGS
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#
navigator start
#
# Sensors, Logging, GPS
#
@@ -550,6 +540,16 @@ then
fi
fi
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#
navigator start
#
# Generic setup (autostart ID not found)
#
Tools/fix_code_style.sh
+2 -1
View File
@@ -16,5 +16,6 @@ astyle \
--ignore-exclude-errors-x \
--lineend=linux \
--exclude=EASTL \
--add-brackets \
--add-brackets \
--max-code-length=120 \
$*
nuttx-configs/px4fmu-v1/nsh/defconfig
+6 -6
View File
@@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y
#
# USART1 Configuration
#
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART1_RXBUFSIZE=256
CONFIG_USART1_TXBUFSIZE=128
CONFIG_USART1_BAUD=57600
CONFIG_USART1_BITS=8
CONFIG_USART1_PARITY=0
@@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y
#
# UART5 Configuration
#
CONFIG_UART5_RXBUFSIZE=512
CONFIG_UART5_TXBUFSIZE=512
CONFIG_UART5_RXBUFSIZE=256
CONFIG_UART5_TXBUFSIZE=128
CONFIG_UART5_BAUD=57600
CONFIG_UART5_BITS=8
CONFIG_UART5_PARITY=0
@@ -540,8 +540,8 @@ CONFIG_UART5_2STOP=0
#
# USART6 Configuration
#
CONFIG_USART6_RXBUFSIZE=512
CONFIG_USART6_TXBUFSIZE=512
CONFIG_USART6_RXBUFSIZE=128
CONFIG_USART6_TXBUFSIZE=64
CONFIG_USART6_BAUD=57600
CONFIG_USART6_BITS=8
CONFIG_USART6_PARITY=0
nuttx-configs/px4io-v2/include/board.h
-2
View File
@@ -103,8 +103,6 @@
#define GPIO_USART2_RTS 0xffffffff
#undef GPIO_USART2_CK
#define GPIO_USART2_CK 0xffffffff
#undef GPIO_USART3_TX
#define GPIO_USART3_TX 0xffffffff
#undef GPIO_USART3_CK
#define GPIO_USART3_CK 0xffffffff
#undef GPIO_USART3_CTS
nuttx-configs/px4io-v2/nsh/defconfig
+2 -2
View File
@@ -104,9 +104,9 @@ CONFIG_ARMV7M_CMNVECTOR=y
# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
#
CONFIG_STM32_DFU=n
CONFIG_STM32_JTAG_FULL_ENABLE=y
CONFIG_STM32_JTAG_FULL_ENABLE=n
CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
CONFIG_STM32_JTAG_SW_ENABLE=n
CONFIG_STM32_JTAG_SW_ENABLE=y
#
# Individual subsystems can be enabled:
src/drivers/blinkm/blinkm.cpp
+43 -28
View File
@@ -194,6 +194,26 @@ private:
bool systemstate_run;
int vehicle_status_sub_fd;
int vehicle_control_mode_sub_fd;
int vehicle_gps_position_sub_fd;
int actuator_armed_sub_fd;
int safety_sub_fd;
int num_of_cells;
int detected_cells_runcount;
int t_led_color[8];
int t_led_blink;
int led_thread_runcount;
int led_interval;
bool topic_initialized;
bool detected_cells_blinked;
bool led_thread_ready;
int num_of_used_sats;
void setLEDColor(int ledcolor);
static void led_trampoline(void *arg);
void led();
@@ -265,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) :
led_color_7(LED_OFF),
led_color_8(LED_OFF),
led_blink(LED_NOBLINK),
systemstate_run(false)
systemstate_run(false),
vehicle_status_sub_fd(-1),
vehicle_control_mode_sub_fd(-1),
vehicle_gps_position_sub_fd(-1),
actuator_armed_sub_fd(-1),
safety_sub_fd(-1),
num_of_cells(0),
detected_cells_runcount(0),
t_led_color({0}),
t_led_blink(0),
led_thread_runcount(0),
led_interval(1000),
topic_initialized(false),
detected_cells_blinked(false),
led_thread_ready(true),
num_of_used_sats(0)
{
memset(&_work, 0, sizeof(_work));
}
@@ -382,31 +417,6 @@ void
BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
static int actuator_armed_sub_fd;
static int safety_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
static int led_thread_runcount=0;
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_control_mode = 0;
static int no_data_actuator_armed = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
static bool detected_cells_blinked = false;
static bool led_thread_ready = true;
int num_of_used_sats = 0;
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 250);
@@ -494,6 +504,11 @@ BlinkM::led()
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
int no_data_vehicle_status = 0;
int no_data_vehicle_control_mode = 0;
int no_data_actuator_armed = 0;
int no_data_vehicle_gps_position = 0;
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
@@ -638,11 +653,11 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
src/drivers/hil/hil.cpp
+1 -1
View File
@@ -122,7 +122,7 @@ private:
actuator_controls_s _controls;
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
src/drivers/hmc5883/hmc5883.cpp
+6 -4
View File
@@ -158,6 +158,7 @@ private:
int _class_instance;
orb_advert_t _mag_topic;
orb_advert_t _subsystem_pub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -324,7 +325,9 @@ HMC5883::HMC5883(int bus) :
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_mag_topic(-1),
_subsystem_pub(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
@@ -1137,13 +1140,12 @@ int HMC5883::check_calibration()
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
static orb_advert_t pub = -1;
if (!(_pub_blocked)) {
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
if (_subsystem_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}
src/drivers/hott/messages.cpp
+17 -15
View File
@@ -51,6 +51,8 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
@@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
static int _esc_sub = -1;
static orb_advert_t _esc_pub;
struct esc_status_s _esc;
static bool _home_position_set = false;
static double _home_lat = 0.0d;
@@ -82,8 +83,6 @@ init_sub_messages(void)
void
init_pub_messages(void)
{
memset(&_esc, 0, sizeof(_esc));
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
}
void
@@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
size_t size = sizeof(msg);
memset(&msg, 0, size);
memcpy(&msg, buffer, size);
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
// Publish it.
esc.timestamp = hrt_absolute_time();
esc.esc_count = 1;
esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
esc.esc[0].esc_temperature = msg.temperature1 - 20;
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
/* announce the esc if needed, just publish else */
if (_esc_pub > 0) {
orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
} else {
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
}
// Publish it.
_esc.esc_count = 1;
_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
_esc.esc[0].esc_temperature = msg.temperature1 - 20;
_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
}
void
src/drivers/mkblctrl/mkblctrl.cpp
+17 -21
View File
@@ -92,8 +92,20 @@
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
class MK : public device::I2C
{
@@ -154,8 +166,10 @@ private:
actuator_controls_s _controls;
MotorData_t Motor[MAX_MOTORS];
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
@@ -195,24 +209,6 @@ const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0};
int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
struct MotorData_t {
unsigned int Version; // the version of the BL (0 = old)
unsigned int SetPoint; // written by attitude controller
unsigned int SetPointLowerBits; // for higher Resolution of new BLs
float SetPoint_PX4; // Values from PX4
unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
unsigned int ReadMode; // select data to read
unsigned short RawPwmValue; // length of PWM pulse
// the following bytes must be exactly in that order!
unsigned int Current; // in 0.1 A steps, read back from BL
unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
unsigned int RoundCount;
};
MotorData_t Motor[MAX_MOTORS];
namespace
{
src/drivers/ms5611/ms5611.cpp
+5
View File
@@ -526,6 +526,7 @@ void
MS5611::cycle()
{
int ret;
unsigned dummy;
/* collection phase? */
if (_collect_phase) {
@@ -542,6 +543,8 @@ MS5611::cycle()
} else {
//log("collection error %d", ret);
}
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
return;
@@ -573,6 +576,8 @@ MS5611::cycle()
ret = measure();
if (ret != OK) {
//log("measure error %d", ret);
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
return;
src/drivers/px4fmu/fmu.cpp
+169 -111
View File
@@ -120,19 +120,25 @@ private:
uint32_t _pwm_alt_rate_channels;
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_actuator_armed;
orb_advert_t _t_outputs;
int _armed_sub;
orb_advert_t _outputs_pub;
actuator_armed_s _armed;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
bool _servo_armed;
bool _pwm_on;
MixerGroup *_mixers;
actuator_controls_s _controls;
uint32_t _groups_required;
uint32_t _groups_subscribed;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
pwm_limit_t _pwm_limit;
uint16_t _failsafe_pwm[_max_actuators];
@@ -143,13 +149,13 @@ private:
unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
void task_main();
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
void subscribe();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -216,15 +222,18 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_control_subs({-1}),
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(-1),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_servo_armed(false),
_pwm_on(false),
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
@@ -235,6 +244,14 @@ PX4FMU::PX4FMU() :
_max_pwm[i] = PWM_DEFAULT_MAX;
}
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
_debug_enabled = true;
}
@@ -447,33 +464,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
}
void
PX4FMU::subscribe()
{
/* subscribe/unsubscribe to required actuator control groups */
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
void
PX4FMU::task_main()
{
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* force a reset of the update rate */
_current_update_rate = 0;
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
@@ -491,6 +518,12 @@ PX4FMU::task_main()
/* loop until killed */
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/* force setting update rate */
_current_update_rate = 0;
}
/*
* Adjust actuator topic update rate to keep up with
@@ -515,20 +548,23 @@ PX4FMU::task_main()
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
orb_set_interval(_t_actuators, update_rate_in_ms);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_set_interval(_control_subs[i], update_rate_in_ms);
}
}
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
* input at 50Hz */
int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
} else if (ret == 0) {
@@ -537,89 +573,98 @@ PX4FMU::task_main()
} else {
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
unsigned num_outputs;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
poll_id++;
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* can we mix? */
if (_mixers != nullptr) {
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
unsigned num_outputs;
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
if (_armed != set_armed)
_armed = set_armed;
case MODE_4PWM:
num_outputs = 4;
break;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
case MODE_6PWM:
num_outputs = 6;
break;
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
default:
num_outputs = 0;
break;
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* publish mixed control outputs */
if (_outputs_pub < 0) {
_outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
} else {
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
}
}
}
/* check arming state */
bool updated = false;
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;
if (_servo_armed != set_armed)
_servo_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
@@ -661,8 +706,13 @@ PX4FMU::task_main()
}
::close(_t_actuators);
::close(_t_actuator_armed);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
::close(_armed_sub);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -684,7 +734,7 @@ PX4FMU::control_callback(uintptr_t handle,
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
input = controls[control_group].control[control_index];
return 0;
}
@@ -1053,6 +1103,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
@@ -1061,18 +1112,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
(uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
(uintptr_t)_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
}
break;
@@ -1083,9 +1136,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
@@ -1096,7 +1150,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
src/drivers/px4io/px4io.cpp
+20 -4
View File
@@ -683,6 +683,25 @@ PX4IO::init()
/* send command to arm system via command API */
vehicle_command_s cmd;
/* send this to itself */
param_t sys_id_param = param_find("MAV_SYS_ID");
param_t comp_id_param = param_find("MAV_COMP_ID");
int32_t sys_id;
int32_t comp_id;
if (param_get(sys_id_param, &sys_id)) {
errx(1, "PRM SYSID");
}
if (param_get(comp_id_param, &comp_id)) {
errx(1, "PRM CMPID");
}
cmd.target_system = sys_id;
cmd.target_component = comp_id;
cmd.source_system = sys_id;
cmd.source_component = comp_id;
/* request arming */
cmd.param1 = 1.0f;
cmd.param2 = 0;
@@ -692,10 +711,7 @@ PX4IO::init()
cmd.param6 = 0;
cmd.param7 = 0;
cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
// cmd.target_system = status.system_id;
// cmd.target_component = status.component_id;
// cmd.source_system = status.system_id;
// cmd.source_component = status.component_id;
/* ask to confirm command */
cmd.confirmation = 1;
src/drivers/stm32/drv_hrt.c
+9 -1
View File
@@ -94,7 +94,7 @@
#elif HRT_TIMER == 3
# define HRT_TIMER_BASE STM32_TIM3_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM3
# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN
# if CONFIG_STM32_TIM3
@@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0;
__EXPORT unsigned ppm_decoded_channels = 0;
__EXPORT uint64_t ppm_last_valid_decode = 0;
#define PPM_DEBUG 0
#if PPM_DEBUG
/* PPM edge history */
__EXPORT uint16_t ppm_edge_history[32];
unsigned ppm_edge_next;
@@ -361,6 +364,7 @@ unsigned ppm_edge_next;
/* PPM pulse history */
__EXPORT uint16_t ppm_pulse_history[32];
unsigned ppm_pulse_next;
#endif
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
@@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status)
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
#if PPM_DEBUG
ppm_edge_history[ppm_edge_next++] = width;
if (ppm_edge_next >= 32)
ppm_edge_next = 0;
#endif
/*
* if this looks like a start pulse, then push the last set of values
@@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status)
interval = count - ppm.last_mark;
ppm.last_mark = count;
#if PPM_DEBUG
ppm_pulse_history[ppm_pulse_next++] = interval;
if (ppm_pulse_next >= 32)
ppm_pulse_next = 0;
#endif
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
src/modules/commander/accelerometer_calibration.cpp
+40 -26
View File
@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3,3> board_rotation;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
}
if (old_done_count != done_count)
if (old_done_count != done_count) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done)
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f)
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i])
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 0; // [ g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 1; // [ -g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 2; // [ 0, g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 3; // [ 0, -g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
return 4; // [ 0, 0, g ]
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ]
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
accel_sum[i] += sensor.accelerometer_m_s2[i];
}
count++;
@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
continue;
}
if (errcount > samples_num / 10)
if (errcount > samples_num / 10) {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f)
return ERROR; // Singular matrix
if (det == 0.0f) {
return ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
}
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {
src/modules/commander/airspeed_calibration.cpp
+5 -1
View File
@@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd)
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
@@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
src/modules/commander/commander.cpp
+168 -110
View File
@@ -117,9 +117,10 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 2000000
@@ -220,7 +221,7 @@ 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 arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -232,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
int commander_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
@@ -260,8 +262,9 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running)
if (!thread_running) {
errx(0, "commander already stopped");
}
thread_should_exit = true;
@@ -303,8 +306,9 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
@@ -363,20 +367,22 @@ void print_status()
static orb_advert_t status_pub;
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// 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, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// 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, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
}
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)
@@ -416,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (hil_ret == OK)
if (hil_ret == OK) {
ret = true;
}
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED)
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@@ -434,13 +442,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
@@ -455,8 +463,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
@@ -465,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (main_res == TRANSITION_CHANGED)
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -479,19 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
} else {
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
} else {
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
break;
@@ -519,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
@@ -539,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
if (use_current) {
/* use current position */
if (status->condition_global_position_valid) {
@@ -582,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -595,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
@@ -628,8 +649,8 @@ int commander_thread_main(int argc, char *argv[])
char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL";
main_states_str[1] = "SEATBELT";
main_states_str[2] = "EASY";
main_states_str[1] = "ALTCTL";
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
char *arming_states_str[ARMING_STATE_MAX];
@@ -741,8 +762,9 @@ int commander_thread_main(int argc, char *argv[])
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
uint64_t last_idle_time = 0;
uint64_t start_time = 0;
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
hrt_abstime last_auto_state_valid = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -771,6 +793,9 @@ int commander_thread_main(int argc, char *argv[])
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_s global_position;
memset(&global_position, 0, sizeof(global_position));
/* Init EPH and EPV */
global_position.eph = 1000.0f;
global_position.epv = 1000.0f;
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -873,6 +898,7 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
@@ -913,6 +939,7 @@ 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);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
@@ -930,23 +957,31 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
} else {
eph_epv_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
} else {
eph_epv_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@@ -981,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
@@ -994,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
@@ -1063,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[])
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
if (last_idle_time > 0)
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
if (last_idle_time > 0) {
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
}
last_idle_time = system_load.tasks[0].total_runtime;
@@ -1140,22 +1178,22 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
transition_result_t res; // store all transitions results here
transition_result_t arming_res; // store all transitions results here
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
arming_res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
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);
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@@ -1168,7 +1206,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1177,7 +1215,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@@ -1190,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter = 0;
}
if (res == TRANSITION_CHANGED) {
if (arming_res == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
@@ -1198,24 +1236,24 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
} else if (res == TRANSITION_DENIED) {
} else if (arming_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status, &sp_man);
transition_result_t main_res = set_main_state_rc(&status, &sp_man);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
if (main_res == TRANSITION_CHANGED) {
tune_positive(armed.armed);
} else if (res == TRANSITION_DENIED) {
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
@@ -1228,7 +1266,8 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* MISSION switch */
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
@@ -1240,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
@@ -1257,34 +1296,39 @@ int commander_thread_main(int argc, char *argv[])
if (armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_DENIED) {
if (auto_res == TRANSITION_NOT_CHANGED) {
last_auto_state_valid = hrt_absolute_time();
}
/* still invalid state after the timeout interval, execute failsafe */
if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
if (auto_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
} else {
/* failsafe for manual modes */
transition_result_t res = TRANSITION_DENIED;
transition_result_t manual_res = TRANSITION_DENIED;
if (!status.condition_landed) {
/* vehicle is not landed, try to perform RTL */
res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
}
if (res == TRANSITION_DENIED) {
if (manual_res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_DENIED) {
if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
}
@@ -1292,14 +1336,14 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
}
}
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
/* flight termination in manual mode if assist switch is on POSCTL position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@@ -1313,8 +1357,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
status_changed = true;
}
}
/* check which state machines for changes, clear "changed" flag */
@@ -1331,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
@@ -1353,6 +1398,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
was_armed = armed.armed;
if (main_state_changed) {
@@ -1516,21 +1562,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
if (leds_counter % 20 == 0) {
led_toggle(LED_BLUE);
}
} else {
/* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_BLUE);
}
}
#endif
/* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) {
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_AMBER);
}
} else {
led_off(LED_AMBER);
@@ -1555,26 +1604,26 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_MIDDLE: // ASSISTED
if (sp_man->assisted_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_EASY);
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT
print_reject_mode(status, "EASY");
// else fallback to ALTCTL
print_reject_mode(status, "POSCTL");
}
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->assisted_switch != SWITCH_POS_ON) {
print_reject_mode(status, "SEATBELT");
if (sp_man->posctl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTL");
}
// else fallback to MANUAL
@@ -1589,9 +1638,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
// else fallback to SEATBELT (EASY likely will not work too)
// else fallback to ALTCTL (POSCTL likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -1637,7 +1686,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -1648,7 +1697,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -1744,7 +1793,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
@@ -1794,8 +1843,9 @@ void *commander_low_prio_loop(void *arg)
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for thread_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -1810,8 +1860,9 @@ void *commander_low_prio_loop(void *arg)
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO)
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
continue;
}
/* only handle low-priority commands here */
switch (cmd.command) {
@@ -1889,6 +1940,7 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
@@ -1903,10 +1955,12 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
if (calib_ret == OK) {
tune_positive(true);
else
} else {
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
@@ -1926,11 +1980,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1946,11 +2002,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1960,8 +2018,8 @@ void *commander_low_prio_loop(void *arg)
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
/* handled in the IO driver */
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
src/modules/commander/commander_helper.cpp
+16 -4
View File
@@ -113,17 +113,22 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune(int tune) {
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
@@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
@@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
@@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
@@ -244,22 +252,25 @@ int led_off(int led)
void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
}
}
void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
}
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds != -1)
if (rgbleds != -1) {
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+12 -12
View File
@@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
ut_assert("tranisition: manual to seatbelt",
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
// MANUAL to ALTCTRL, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
new_main_state = MAIN_STATE_ALTCTL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
ut_assert("transition: manual to easy",
new_main_state = MAIN_STATE_POSCTL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
// MANUAL to EASY, invalid local position.
// MANUAL to POSCTRL, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
new_main_state = MAIN_STATE_POSCTL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
src/modules/commander/gyro_calibration.cpp
+10 -8
View File
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
gyro_scale.z_offset += gyro_report.z;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else {
poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
warn("WARNING: failed to apply new offsets for gyro");
}
close(fd);
@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
uint64_t last_time = hrt_absolute_time();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2 * M_PI_F;
if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
if (mag < -M_PI_F) mag += 2 * M_PI_F;
if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2 * M_PI_F;
if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
if (diff < -M_PI_F) diff += 2 * M_PI_F;
if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
baseline_integral += diff;
mag_last = mag;
src/modules/commander/mag_calibration.cpp
+21 -10
View File
@@ -124,6 +124,7 @@ int do_mag_calibration(int mavlink_fd)
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
@@ -163,8 +164,9 @@ int do_mag_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_maxcount / 20) == 0)
if (calibration_counter % (calibration_maxcount / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
}
} else {
poll_errcount++;
@@ -198,14 +200,17 @@ int do_mag_calibration(int mavlink_fd)
}
}
if (x != NULL)
if (x != NULL) {
free(x);
}
if (y != NULL)
if (y != NULL) {
free(y);
}
if (z != NULL)
if (z != NULL) {
free(z);
}
if (res == OK) {
/* apply calibration and set parameters */
@@ -234,23 +239,29 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
src/modules/commander/px4_custom_mode.h
+2 -2
View File
@@ -12,8 +12,8 @@
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
};
src/modules/commander/rc_calibration.cpp
+3 -3
View File
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
float p = sp.y;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
p = sp.x;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
p = sp.r;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
src/modules/commander/state_machine_helper.cpp
+86 -72
View File
@@ -75,38 +75,38 @@ static bool failsafe_state_changed = true;
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char* state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
static const char *state_names[ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
@@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == status->arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED) {
static const char* errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
return ret;
}
@@ -215,7 +222,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
@@ -226,7 +233,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
@@ -320,6 +327,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
@@ -331,26 +339,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
-169
View File
@@ -1,169 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_att.h"
struct fw_att_control_params {
float roll_p;
float rollrate_lim;
float pitch_p;
float pitchrate_lim;
float yawrate_lim;
float pitch_roll_compensation_p;
};
struct fw_pos_control_param_handles {
param_t roll_p;
param_t rollrate_lim;
param_t pitch_p;
param_t pitchrate_lim;
param_t yawrate_lim;
param_t pitch_roll_compensation_p;
};
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->roll_p = param_find("FW_ROLL_P");
h->rollrate_lim = param_find("FW_ROLLR_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitchrate_lim = param_find("FW_PITCHR_LIM");
h->yawrate_lim = param_find("FW_YAWR_LIM");
h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->roll_p, &(p->roll_p));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
return OK;
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
static struct fw_att_control_params p;
static struct fw_pos_control_param_handles h;
static PID_t roll_controller;
static PID_t pitch_controller;
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
counter++;
return 0;
}
src/modules/fixedwing_att_control/fixedwing_att_control_att.h
-51
View File
@@ -1,51 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
*
* 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 Fixed Wing Attitude Control */
#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
#define FIXEDWING_ATT_CONTROL_ATT_H_
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp);
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
-367
View File
@@ -1,367 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_att_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
#include "fixedwing_att_control_att.h"
/* Prototypes */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_att_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/* Main Thread */
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* Check if there is a new position measurement or attitude setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (att_sp_updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
printf("FW ATT CONTROL: Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* set manual setpoints if required */
if (control_mode.flag_control_manual_enabled) {
if (control_mode.flag_control_attitude_enabled) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* execute attitude control if requested */
if (control_mode.flag_control_attitude_enabled) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_att_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_att_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
-211
View File
@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
*
* 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 fixedwing_att_control_rate.c
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Implementation of a fixed wing attitude controller.
*
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include "fixedwing_att_control_rate.h"
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* feedforward compensation */
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
struct fw_rate_control_params {
float rollrate_p;
float rollrate_i;
float rollrate_awu;
float pitchrate_p;
float pitchrate_i;
float pitchrate_awu;
float yawrate_p;
float yawrate_i;
float yawrate_awu;
float pitch_thr_ff;
};
struct fw_rate_control_param_handles {
param_t rollrate_p;
param_t rollrate_i;
param_t rollrate_awu;
param_t pitchrate_p;
param_t pitchrate_i;
param_t pitchrate_awu;
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_awu;
param_t pitch_thr_ff;
};
/* Internal Prototypes */
static int parameters_init(struct fw_rate_control_param_handles *h);
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
static int parameters_init(struct fw_rate_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
return OK;
}
static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
{
param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
return OK;
}
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
{
static int counter = 0;
static bool initialized = false;
static struct fw_rate_control_params p;
static struct fw_rate_control_param_handles h;
static PID_t roll_rate_controller;
static PID_t pitch_rate_controller;
static PID_t yaw_rate_controller;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
}
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
counter++;
return 0;
}
src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
-48
View File
@@ -1,48 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
*
* 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 Fixed Wing Attitude Rate Control */
#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
#define FIXEDWING_ATT_CONTROL_RATE_H_
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators);
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
src/modules/fixedwing_att_control/module.mk
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 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.
#
############################################################################
#
# Fixedwing Attitude Control application
#
MODULE_COMMAND = fixedwing_att_control
SRCS = fixedwing_att_control_main.c \
fixedwing_att_control_att.c \
fixedwing_att_control_rate.c
src/modules/fixedwing_backside/fixedwing.cpp
+2 -2
View File
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
-479
View File
@@ -1,479 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_pos_control.c
* Implementation of a fixed wing attitude controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
struct fw_pos_control_params {
float heading_p;
float headingr_p;
float headingr_i;
float headingr_lim;
float xtrack_p;
float altitude_p;
float roll_lim;
float pitch_lim;
};
struct fw_pos_control_param_handles {
param_t heading_p;
param_t headingr_p;
param_t headingr_i;
param_t headingr_lim;
param_t xtrack_p;
param_t altitude_p;
param_t roll_lim;
param_t pitch_lim;
};
struct planned_path_segments_s {
bool segment_type;
double start_lat; // Start of line or center of arc
double start_lon;
double end_lat;
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
/* Prototypes */
/* Internal Prototypes */
static int parameters_init(struct fw_pos_control_param_handles *h);
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
/**
* Deamon management function.
*/
__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/* Variables */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Parameter management
*/
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->heading_p = param_find("FW_HEAD_P");
h->headingr_p = param_find("FW_HEADR_P");
h->headingr_i = param_find("FW_HEADR_I");
h->headingr_lim = param_find("FW_HEADR_LIM");
h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->heading_p, &(p->heading_p));
param_get(h->headingr_p, &(p->headingr_p));
param_get(h->headingr_i, &(p->headingr_i));
param_get(h->headingr_lim, &(p->headingr_lim));
param_get(h->xtrack_p, &(p->xtrack_p));
param_get(h->altitude_p, &(p->altitude_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
/* Main Thread */
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint;
memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* publish attitude setpoint */
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false;
float psi_track = 0.0f;
int counter = 0;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t heading_rate_controller;
PID_t offtrack_controller;
PID_t altitude_controller;
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
}
/* Simple Horizontal Control */
if (global_sp_updated_set_once) {
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi(psi_e);
if (verbose) {
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
printf("delta_psi_c %.4f ", (double)delta_psi_c);
printf("psi_c %.4f ", (double)psi_c);
printf("att.yaw %.4f ", (double)att.yaw);
printf("psi_e %.4f ", (double)psi_e);
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
/* limit turn rate */
if (psi_rate_c > p.headingr_lim) {
psi_rate_c = p.headingr_lim;
} else if (psi_rate_c < -p.headingr_lim) {
psi_rate_c = -p.headingr_lim;
}
float psi_rate_e = psi_rate_c - att.yawspeed;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
if (ground_speed < 10.0f) {
ground_speed = 10.0f;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
}
if (verbose && counter % 100 == 0)
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
} else {
if (verbose && counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
/* Very simple Altitude Control */
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
counter++;
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
}
}
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
close(attitude_setpoint_pub);
fflush(stdout);
exit(0);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_pos_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_pos_control is running\n");
} else {
printf("\tfixedwing_pos_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
src/modules/fixedwing_pos_control/module.mk
-40
View File
@@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 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.
#
############################################################################
#
# Fixedwing PositionControl application
#
MODULE_COMMAND = fixedwing_pos_control
SRCS = fixedwing_pos_control_main.c
src/modules/fw_att_control/fw_att_control_main.cpp
+13 -12
View File
@@ -273,7 +273,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace att_control
@@ -706,20 +706,21 @@ FixedwingAttitudeControl::task_main()
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do 45 degrees, the mapping is
* -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
* and a typical remote can only do around 45 degrees, the mapping is
* -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote.
* the commanded attitude.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
throttle_sp = _manual.throttle;
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+ _parameters.pitchsp_offset_rad;
throttle_sp = _manual.z;
_actuators.control[4] = _manual.flaps;
/*
@@ -825,10 +826,10 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.roll;
_actuators.control[1] = -_manual.pitch;
_actuators.control[2] = _manual.yaw;
_actuators.control[3] = _manual.throttle;
_actuators.control[0] = _manual.y;
_actuators.control[1] = -_manual.x;
_actuators.control[2] = _manual.r;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
}
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+1 -1
View File
@@ -237,7 +237,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace estimator
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+136 -63
View File
@@ -89,6 +89,7 @@
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
@@ -135,6 +136,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
@@ -148,13 +150,14 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
@@ -182,7 +185,7 @@ private:
/* Landingslope object */
Landingslope landingslope;
float flare_curve_alt_last;
float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
@@ -242,6 +245,7 @@ private:
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
float range_finder_rel_alt;
} _parameters; /**< local copies of interesting parameters */
@@ -286,6 +290,7 @@ private:
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
param_t range_finder_rel_alt;
} _parameter_handles; /**< handles for interesting parameters */
@@ -311,6 +316,12 @@ private:
*/
bool vehicle_airspeed_poll();
/**
* Check for range finder updates.
*/
bool range_finder_poll();
/**
* Check for position updates.
*/
@@ -331,6 +342,11 @@ private:
*/
void navigation_capabilities_publish();
/**
* Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
*/
float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
/**
* Control position.
*/
@@ -348,7 +364,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
/*
* Reset takeoff state
@@ -368,6 +384,7 @@ private:
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
@@ -399,6 +416,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
@@ -418,7 +436,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
launch_detected(false),
last_manual(false),
usePreTakeoffThrust(false),
flare_curve_alt_last(0.0f),
flare_curve_alt_rel_last(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
@@ -434,7 +452,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_pos_sp_triplet(),
_sensor_combined(),
_mTecs(),
_was_pos_control_mode(false)
_was_pos_control_mode(false),
_range_finder()
{
_nav_capabilities.turn_distance = 0.0f;
@@ -459,6 +478,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -548,6 +568,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -646,6 +668,20 @@ FixedwingPositionControl::vehicle_airspeed_poll()
return airspeed_updated;
}
bool
FixedwingPositionControl::range_finder_poll()
{
/* check if there is a range finder measurement */
bool range_finder_updated;
orb_check(_range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
}
return range_finder_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -774,6 +810,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
{
float rel_alt_estimated = current_alt - land_setpoint_alt;
/* only use range finder if:
* parameter (range_finder_use_relative_alt) > 0
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
return rel_alt_estimated;
}
return range_finder.distance;
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -852,7 +905,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), ground_speed);
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
@@ -865,7 +918,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), ground_speed);
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
@@ -924,12 +977,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
@@ -939,7 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -948,19 +1003,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
/* avoid climbout */
if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
{
flare_curve_alt = pos_sp_triplet.current.alt;
flare_curve_alt_rel = 0.0f; // stay on ground
land_stayonground = true;
}
tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas,
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
flare_pitch_angle_rad, math::radians(15.0f),
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -968,7 +1026,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
flare_curve_alt_last = flare_curve_alt;
flare_curve_alt_rel_last = flare_curve_alt_rel;
} else {
/* intersect glide slope:
@@ -976,22 +1034,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* */
float altitude_desired = _global_pos.alt;
if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
/* stay on slope */
altitude_desired = landing_slope_alt_desired;
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
land_onslope = true;
}
} else {
/* continue horizontally */
altitude_desired = math::max(_global_pos.alt, L_altitude);
altitude_desired_rel = math::max(relative_alt, L_altitude_rel);
}
tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas,
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_pos_sp_triplet.current.alt + relative_alt,
false, math::radians(_parameters.pitch_limit_min), ground_speed);
}
@@ -1030,10 +1089,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1042,7 +1110,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), ground_speed);
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
}
} else {
@@ -1074,23 +1142,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** EASY FLIGHT **/
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
}
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
//XXX not used
/* climb out control */
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
@@ -1098,44 +1166,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// climb_out = true;
// }
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), ground_speed);
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* seatbelt mode enabled */) {
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** SEATBELT FLIGHT **/
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to seatbelt */) {
_seatbelt_hold_heading = _att.yaw;
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* seatbelt on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_manual.z;
/* user switched off throttle */
if (_manual.throttle < 0.1f) {
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
@@ -1145,17 +1214,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min), ground_speed);
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
@@ -1210,6 +1280,7 @@ FixedwingPositionControl::task_main()
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
@@ -1289,6 +1360,7 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
@@ -1357,6 +1429,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
fwPosctrl::mTecs::tecs_mode mode)
{
@@ -1366,9 +1439,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+11
View File
@@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
/**
* Relative altitude threshold for range finder measurements for use during landing
*
* range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
* set to < 0 to disable
* the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
src/modules/fw_pos_control_l1/landingslope.cpp
+27 -7
View File
@@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues()
_horizontal_slope_displacement = (_flare_length - _d1);
}
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
{
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
return getLandingSlopeRelativeAltitude(wp_landing_distance);
else
return 0.0f;
}
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
{
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
{
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
else
return wp_altitude;
}
float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
else
return wp_landing_altitude;
return 0.0f;
}
float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
{
return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
}
src/modules/fw_pos_control_l1/landingslope.h
+28 -3
View File
@@ -63,11 +63,26 @@ public:
Landingslope() {}
~Landingslope() {}
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeRelativeAltitude(float wp_landing_distance);
/**
*
* @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
* Performs check if aircraft is in front of waypoint to avoid climbout
*/
float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude);
/**
*
@@ -76,13 +91,22 @@ public:
*/
float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
/**
*
* @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
}
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
}
/**
@@ -95,8 +119,9 @@ public:
}
float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
src/modules/mavlink/mavlink_main.cpp
+4 -2
View File
@@ -727,9 +727,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
static mavlink_message_t tx_msg;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
@@ -1535,6 +1535,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
statustext.severity = MAV_SEVERITY_INFO;
int i = 0;
while (i < len - 1) {
src/modules/mavlink/mavlink_messages.cpp
+26 -25
View File
@@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) {
} else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_EASY) {
} else if (status->main_state == MAIN_STATE_POSCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -819,11 +819,11 @@ protected:
void send(const hrt_abstime t)
{
bool updated = status_sub->update(t);
updated |= pos_sp_triplet_sub->update(t);
updated |= act_sub->update(t);
bool updated = act_sub->update(t);
(void)pos_sp_triplet_sub->update(t);
(void)status_sub->update(t);
if (updated) {
if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@@ -1138,10 +1138,10 @@ protected:
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
manual->x * 1000,
manual->y * 1000,
manual->z * 1000,
manual->r * 1000,
0);
}
}
@@ -1339,22 +1339,23 @@ protected:
void send(const hrt_abstime t)
{
(void)range_sub->update(t);
if (range_sub->update(t)) {
uint8_t type;
uint8_t type;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
switch (range->type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
uint8_t id = 0;
uint8_t orientation = 0;
uint8_t covariance = 20;
mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
}
};
src/modules/mavlink/mavlink_receiver.cpp
+11 -5
View File
@@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
}
}
/* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true);
@@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
@@ -432,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
memset(&manual, 0, sizeof(manual));
manual.timestamp = hrt_absolute_time();
manual.pitch = man.x / 1000.0f;
manual.roll = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
src/modules/mc_att_control/mc_att_control_main.cpp
+23 -12
View File
@@ -71,7 +71,7 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
/**
@@ -156,6 +156,7 @@ private:
param_t yaw_rate_i;
param_t yaw_rate_d;
param_t yaw_ff;
param_t yaw_rate_max;
param_t man_roll_max;
param_t man_pitch_max;
@@ -168,6 +169,7 @@ private:
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float yaw_rate_max; /**< max yaw rate */
float man_roll_max;
float man_pitch_max;
@@ -225,9 +227,9 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main attitude control task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace mc_att_control
@@ -276,6 +278,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_rates_prev.zero();
_rates_sp.zero();
@@ -298,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
@@ -367,15 +374,16 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
/* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max *= M_PI / 180.0;
_params.man_pitch_max *= M_PI / 180.0;
_params.man_yaw_max *= M_PI / 180.0;
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
return OK;
}
@@ -478,7 +486,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp.thrust = _manual_control_sp.throttle;
_v_att_sp.thrust = _manual_control_sp.z;
publish_att_sp = true;
}
@@ -496,7 +504,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max;
yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
@@ -512,8 +520,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
@@ -626,6 +634,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
src/modules/mc_att_control/mc_att_control_params.c
+12
View File
@@ -174,6 +174,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
/**
* Max manual roll
*
src/modules/mc_pos_control/mc_pos_control_main.cpp
+5 -5
View File
@@ -226,7 +226,7 @@ private:
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
void task_main();
};
namespace pos_control
@@ -617,7 +617,7 @@ MulticopterPositionControl::task_main()
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
@@ -625,8 +625,8 @@ MulticopterPositionControl::task_main()
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _manual.pitch;
sp_move_rate(1) = _manual.roll;
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
@@ -782,7 +782,7 @@ MulticopterPositionControl::task_main()
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.throttle;
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
src/modules/mc_pos_control/mc_pos_control_params.c
+5 -5
View File
@@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
/**
* Maximum vertical velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
*
* @unit m/s
* @min 0.0
@@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
/**
* Maximum horizontal velocity
*
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.0
@@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
/**
* Maximum tilt angle in air
*
* Limits maximum tilt in AUTO and EASY modes during flight.
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @min 0.0
src/modules/navigator/mission_feasibility_checker.cpp
+4 -4
View File
@@ -100,8 +100,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -125,8 +125,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
src/modules/navigator/navigator_main.cpp
+2 -2
View File
@@ -1291,7 +1291,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
@@ -1351,7 +1351,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
_mission_item.pitch_min = 0.0f;
src/modules/position_estimator/module.mk
-44
View File
@@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 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.
#
############################################################################
#
# Makefile to build the position estimator
#
MODULE_COMMAND = position_estimator
# XXX this should be converted to a deamon, its a pretty bad example app
MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT
MODULE_STACKSIZE = 4096
SRCS = position_estimator_main.c
src/modules/position_estimator/position_estimator_main.c
-423
View File
@@ -1,423 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 position_estimator_main.c
* Model-identification based position estimator for multirotors
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <poll.h>
#define N_STATES 6
#define ERROR_COVARIANCE_INIT 3
#define R_EARTH 6371000.0
#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
#define REPROJECTION_COUNTER_LIMIT 125
__EXPORT int position_estimator_main(int argc, char *argv[]);
static uint16_t position_estimator_counter_position_information;
/* values for map projection */
static double phi_1;
static double sin_phi_1;
static double cos_phi_1;
static double lambda_0;
static double scale;
/**
* Initializes the map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
phi_1 = lat_0 / 180.0 * M_PI;
lambda_0 = lon_0 / 180.0 * M_PI;
sin_phi_1 = sin(phi_1);
cos_phi_1 = cos(phi_1);
/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
const double r_earth = 6371000;
double lat1 = phi_1;
double lon1 = lambda_0;
double lat2 = phi_1 + 0.5 / 180 * M_PI;
double lon2 = lambda_0 + 0.5 / 180 * M_PI;
double sin_lat_2 = sin(lat2);
double cos_lat_2 = cos(lat2);
double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
/* 2) calculate distance rho on plane */
double k_bar = 0;
double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
if (0 != c)
k_bar = c / sin(c);
double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
double rho = sqrt(pow(x2, 2) + pow(y2, 2));
scale = d / rho;
}
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_project(double lat, double lon, float *x, float *y)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double phi = lat / 180.0 * M_PI;
double lambda = lon / 180.0 * M_PI;
double sin_phi = sin(phi);
double cos_phi = cos(phi);
double k_bar = 0;
/* using small angle approximation (formula in comment is without aproximation) */
double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
if (0 != c)
k_bar = c / sin(c);
/* using small angle approximation (formula in comment is without aproximation) */
*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
}
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
static void map_projection_reproject(float x, float y, double *lat, double *lon)
{
/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
double x_descaled = x / scale;
double y_descaled = y / scale;
double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
double sin_c = sin(c);
double cos_c = cos(c);
double lat_sphere = 0;
if (c != 0)
lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
else
lat_sphere = asin(cos_c * sin_phi_1);
// printf("lat_sphere = %.10f\n",lat_sphere);
double lon_sphere = 0;
if (phi_1 == M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
} else if (phi_1 == -M_PI / 2) {
//using small angle approximation (formula in comment is without aproximation)
lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
} else {
lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
//using small angle approximation
// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
// if(denominator != 0)
// {
// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
// }
// else
// {
// ...
// }
}
// printf("lon_sphere = %.10f\n",lon_sphere);
*lat = lat_sphere * 180.0 / M_PI;
*lon = lon_sphere * 180.0 / M_PI;
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_main(int argc, char *argv[])
{
/* welcome user */
printf("[multirotor position_estimator] started\n");
/* initialize values */
static float u[2] = {0, 0};
static float z[3] = {0, 0, 0};
static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0};
static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0
};
static float xapo1[N_STATES];
static float Papo1[36];
static float gps_covariance[3] = {0.0f, 0.0f, 0.0f};
static uint16_t counter = 0;
position_estimator_counter_position_information = 0;
uint8_t predict_only = 1;
bool gps_valid = false;
bool new_initialization = true;
static double lat_current = 0.0d;//[°]] --> 47.0
static double lon_current = 0.0d; //[°]] -->8.5
float alt_current = 0.0f;
//TODO: handle flight without gps but with estimator
/* subscribe to vehicle status, attitude, gps */
struct vehicle_gps_position_s gps;
gps.fix_type = 0;
struct vehicle_status_s vstatus;
struct vehicle_attitude_s att;
int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to attitude at 100 Hz */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* wait until gps signal turns valid, only then can we initialize the projection */
while (gps.fix_type < 3) {
struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
* this choice is critical, since the vehicle status might not
* actually change, if this app is started after GPS lock was
* aquired.
*/
if (poll(fds, 1, 5000)) {
/* Wait for the GPS update to propagate (we have some time) */
usleep(5000);
/* Read wether the vehicle status changed */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
gps_valid = (gps.fix_type > 2);
}
}
/* get gps value for first initialization */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
lat_current = ((double)(gps.lat)) * 1e-7;
lon_current = ((double)(gps.lon)) * 1e-7;
alt_current = gps.alt * 1e-3;
/* initialize coordinates */
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
struct vehicle_local_position_s local_pos = {
.x = 0,
.y = 0,
.z = 0
};
orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
while (1) {
/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} };
if (poll(fds, 1, 5000) <= 0) {
/* error / timeout */
} else {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
/* got attitude, updating pos as well */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/*copy attitude */
u[0] = att.roll;
u[1] = att.pitch;
/* initialize map projection with the last estimate (not at full rate) */
if (gps.fix_type > 2) {
/* Project gps lat lon (Geographic coordinate system) to plane*/
map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
local_pos.x = z[0];
local_pos.y = z[1];
/* negative offset from initialization altitude */
local_pos.z = alt_current - (gps.alt) * 1e-3;
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}
// gps_covariance[0] = gps.eph; //TODO: needs scaling
// gps_covariance[1] = gps.eph;
// gps_covariance[2] = gps.epv;
// } else {
// /* we can not use the gps signal (it is of low quality) */
// predict_only = 1;
// }
// // predict_only = 0; //TODO: only for testing, removeme, XXX
// // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX
// // usleep(100000); //TODO: only for testing, removeme, XXX
// /*Get new estimation (this is calculated in the plane) */
// //TODO: if new_initialization == true: use 0,0,0, else use xapo
// if (true == new_initialization) { //TODO,XXX: uncomment!
// xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
// xapo[2] = 0;
// xapo[4] = 0;
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
// } else {
// position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
// }
// /* Copy values from xapo1 to xapo */
// int i;
// for (i = 0; i < N_STATES; i++) {
// xapo[i] = xapo1[i];
// }
// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
// /* Reproject from plane to geographic coordinate system */
// // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
// map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
// // //DEBUG
// // if(counter%500 == 0)
// // {
// // printf("phi_1: %.10f\n", phi_1);
// // printf("lambda_0: %.10f\n", lambda_0);
// // printf("lat_estimated: %.10f\n", lat_current);
// // printf("lon_estimated: %.10f\n", lon_current);
// // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
// // fflush(stdout);
// //
// // }
// // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
// // {
// /* send out */
// global_pos.lat = lat_current;
// global_pos.lon = lon_current;
// global_pos.alt = xapo1[4];
// global_pos.vx = xapo1[1];
// global_pos.vy = xapo1[3];
// global_pos.vz = xapo1[5];
/* publish current estimate */
// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
// }
// else
// {
// printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
// fflush(stdout);
// }
// }
counter++;
}
}
return 0;
}
src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
-58
View File
@@ -1,58 +0,0 @@
/*
* kalman_dlqe1.c
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
const real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
real32_T y;
int32_T i0;
real32_T b_y[3];
int32_T i1;
real32_T f0;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += C[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + K[i0] * y;
}
//printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
}
/* End of code generation (kalman_dlqe1.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_H__
#define __KALMAN_DLQE1_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe1.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe1_initialize.c
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe1_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1_initialize.h
*
* Code generation for function 'kalman_dlqe1_initialize'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_INITIALIZE_H__
#define __KALMAN_DLQE1_INITIALIZE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_initialize(void);
#endif
/* End of code generation (kalman_dlqe1_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe1_terminate.c
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe1.h"
#include "kalman_dlqe1_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe1_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe1_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
-30
View File
@@ -1,30 +0,0 @@
/*
* kalman_dlqe1_terminate.h
*
* Code generation for function 'kalman_dlqe1_terminate'
*
* C source code generated on: Wed Feb 13 20:34:32 2013
*
*/
#ifndef __KALMAN_DLQE1_TERMINATE_H__
#define __KALMAN_DLQE1_TERMINATE_H__
/* Include files */
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "kalman_dlqe1_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe1_terminate(void);
#endif
/* End of code generation (kalman_dlqe1_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe1_types.h
*
* Code generation for function 'kalman_dlqe1'
*
* C source code generated on: Wed Feb 13 20:34:31 2013
*
*/
#ifndef __KALMAN_DLQE1_TYPES_H__
#define __KALMAN_DLQE1_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe1_types.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
-119
View File
@@ -1,119 +0,0 @@
/*
* kalman_dlqe2.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T
x_aposteriori[3])
{
//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
real32_T A[9];
real32_T y;
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T b_k1[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
b_k1[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_k1[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
}
/* End of code generation (kalman_dlqe2.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __KALMAN_DLQE2_H__
#define __KALMAN_DLQE2_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe2.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe2_initialize.c
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (kalman_dlqe2_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2_initialize.h
*
* Code generation for function 'kalman_dlqe2_initialize'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_INITIALIZE_H__
#define __KALMAN_DLQE2_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_initialize(void);
#endif
/* End of code generation (kalman_dlqe2_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe2_terminate.c
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe2.h"
#include "kalman_dlqe2_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe2_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe2_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe2_terminate.h
*
* Code generation for function 'kalman_dlqe2_terminate'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TERMINATE_H__
#define __KALMAN_DLQE2_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe2_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe2_terminate(void);
#endif
/* End of code generation (kalman_dlqe2_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe2_types.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:28 2013
*
*/
#ifndef __KALMAN_DLQE2_TYPES_H__
#define __KALMAN_DLQE2_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe2_types.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
-137
View File
@@ -1,137 +0,0 @@
/*
* kalman_dlqe3.c
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "randn.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f1;
real32_T f2;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f1 = (real32_T)fabs(u0);
f2 = (real32_T)fabs(u1);
if (rtIsInfF(u1)) {
if (f1 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f1 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f2 == 0.0F) {
y = 1.0F;
} else if (f2 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrt(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)pow(u0, u1);
}
}
return y;
}
void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
{
real32_T A[9];
int32_T i0;
static const int8_T iv0[3] = { 0, 0, 1 };
real_T b;
real32_T y;
real32_T b_y[3];
int32_T i1;
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T b_k1[3];
real32_T f0;
A[0] = 1.0F;
A[3] = dt;
A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = dt;
for (i0 = 0; i0 < 3; i0++) {
A[2 + 3 * i0] = (real32_T)iv0[i0];
}
if (addNoise == 1.0F) {
b = randn();
z += sigma * (real32_T)b;
}
if (posUpdate != 0.0F) {
y = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
b_y[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
}
y += b_y[i0] * x_aposteriori_k[i0];
}
y = z - y;
b_k1[0] = k1;
b_k1[1] = k2;
b_k1[2] = k3;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
x_aposteriori[i0] = f0 + b_k1[i0] * y;
}
} else {
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
}
}
}
}
/* End of code generation (kalman_dlqe3.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
#ifndef __KALMAN_DLQE3_H__
#define __KALMAN_DLQE3_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
#endif
/* End of code generation (kalman_dlqe3.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
-32
View File
@@ -1,32 +0,0 @@
/*
* kalman_dlqe3_data.c
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
uint32_T method;
uint32_T state[2];
uint32_T b_method;
uint32_T b_state;
uint32_T c_state[2];
boolean_T state_not_empty;
/* Function Declarations */
/* Function Definitions */
/* End of code generation (kalman_dlqe3_data.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
-38
View File
@@ -1,38 +0,0 @@
/*
* kalman_dlqe3_data.h
*
* Code generation for function 'kalman_dlqe3_data'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_DATA_H__
#define __KALMAN_DLQE3_DATA_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
extern uint32_T method;
extern uint32_T state[2];
extern uint32_T b_method;
extern uint32_T b_state;
extern uint32_T c_state[2];
extern boolean_T state_not_empty;
/* Variable Definitions */
/* Function Declarations */
#endif
/* End of code generation (kalman_dlqe3_data.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
-47
View File
@@ -1,47 +0,0 @@
/*
* kalman_dlqe3_initialize.c
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_initialize.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_initialize(void)
{
int32_T i;
static const uint32_T uv0[2] = { 362436069U, 0U };
rt_InitInfAndNaN(8U);
state_not_empty = FALSE;
b_state = 1144108930U;
b_method = 7U;
method = 0U;
for (i = 0; i < 2; i++) {
c_state[i] = 362436069U + 158852560U * (uint32_T)i;
state[i] = uv0[i];
}
if (state[1] == 0U) {
state[1] = 521288629U;
}
}
/* End of code generation (kalman_dlqe3_initialize.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3_initialize.h
*
* Code generation for function 'kalman_dlqe3_initialize'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_INITIALIZE_H__
#define __KALMAN_DLQE3_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_initialize(void);
#endif
/* End of code generation (kalman_dlqe3_initialize.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* kalman_dlqe3_terminate.c
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "kalman_dlqe3_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void kalman_dlqe3_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (kalman_dlqe3_terminate.c) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
-33
View File
@@ -1,33 +0,0 @@
/*
* kalman_dlqe3_terminate.h
*
* Code generation for function 'kalman_dlqe3_terminate'
*
* C source code generated on: Tue Feb 19 15:26:31 2013
*
*/
#ifndef __KALMAN_DLQE3_TERMINATE_H__
#define __KALMAN_DLQE3_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void kalman_dlqe3_terminate(void);
#endif
/* End of code generation (kalman_dlqe3_terminate.h) */
src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* kalman_dlqe3_types.h
*
* Code generation for function 'kalman_dlqe3'
*
* C source code generated on: Tue Feb 19 15:26:30 2013
*
*/
#ifndef __KALMAN_DLQE3_TYPES_H__
#define __KALMAN_DLQE3_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (kalman_dlqe3_types.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
-136
View File
@@ -1,136 +0,0 @@
/*
* positionKalmanFilter1D.c
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
real32_T P_aposteriori[9])
{
int32_T i0;
real32_T f0;
int32_T k;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
real32_T y;
real32_T K[3];
real32_T S;
int8_T I[9];
/* prediction */
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
}
x_aposteriori[i0] = f0 + B[i0] * u;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
}
P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
y = 0.0F;
for (k = 0; k < 3; k++) {
y += C[k] * x_aposteriori[k];
K[k] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
K[k] += C[i0] * P_apriori[i0 + 3 * k];
}
}
y = z - y;
S = 0.0F;
for (k = 0; k < 3; k++) {
S += K[k] * C[k];
}
S += R;
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (k = 0; k < 3; k++) {
f0 += P_apriori[i0 + 3 * k] * C[k];
}
K[i0] = f0 / S;
}
for (i0 = 0; i0 < 3; i0++) {
x_aposteriori[i0] += K[i0] * y;
}
for (i0 = 0; i0 < 9; i0++) {
I[i0] = 0;
}
for (k = 0; k < 3; k++) {
I[k + 3 * k] = 1;
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
}
}
for (i0 = 0; i0 < 3; i0++) {
for (k = 0; k < 3; k++) {
P_aposteriori[i0 + 3 * k] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
}
}
}
} else {
for (i0 = 0; i0 < 9; i0++) {
P_aposteriori[i0] = P_apriori[i0];
}
}
}
/* End of code generation (positionKalmanFilter1D.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D.h
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_H__
#define __POSITIONKALMANFILTER1D_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
#endif
/* End of code generation (positionKalmanFilter1D.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
-157
View File
@@ -1,157 +0,0 @@
/*
* positionKalmanFilter1D_dT.c
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
x_aposteriori[3], real32_T P_aposteriori[9])
{
real32_T A[9];
int32_T i;
static const int8_T iv0[3] = { 0, 0, 1 };
real32_T K[3];
real32_T f0;
int32_T i0;
real32_T b_A[9];
int32_T i1;
real32_T P_apriori[9];
static const int8_T iv1[3] = { 1, 0, 0 };
real32_T fv0[3];
real32_T y;
static const int8_T iv2[3] = { 1, 0, 0 };
real32_T S;
int8_T I[9];
/* dynamics */
A[0] = 1.0F;
A[3] = dT;
A[6] = -0.5F * dT * dT;
A[1] = 0.0F;
A[4] = 1.0F;
A[7] = -dT;
for (i = 0; i < 3; i++) {
A[2 + 3 * i] = (real32_T)iv0[i];
}
/* prediction */
K[0] = 0.5F * dT * dT;
K[1] = dT;
K[2] = 0.0F;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
}
x_aposteriori[i] = f0 + K[i] * u;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
b_A[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
}
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
}
P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
}
}
if ((real32_T)fabs(u) < thresh) {
x_aposteriori[1] *= decay;
}
/* update */
if (gps_update == 1) {
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += (real32_T)iv1[i] * x_aposteriori[i];
fv0[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
}
}
y = z - f0;
f0 = 0.0F;
for (i = 0; i < 3; i++) {
f0 += fv0[i] * (real32_T)iv2[i];
}
S = f0 + (real32_T)R;
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
}
K[i] = f0 / S;
}
for (i = 0; i < 3; i++) {
x_aposteriori[i] += K[i] * y;
}
for (i = 0; i < 9; i++) {
I[i] = 0;
}
for (i = 0; i < 3; i++) {
I[i + 3 * i] = 1;
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
P_aposteriori[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
}
}
}
} else {
for (i = 0; i < 9; i++) {
P_aposteriori[i] = P_apriori[i];
}
}
}
/* End of code generation (positionKalmanFilter1D_dT.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_dT.h
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_H__
#define __POSITIONKALMANFILTER1D_DT_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
#endif
/* End of code generation (positionKalmanFilter1D_dT.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_dT_initialize.c
*
* Code generation for function 'positionKalmanFilter1D_dT_initialize'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
#include "positionKalmanFilter1D_dT_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_dT_initialize.h
*
* Code generation for function 'positionKalmanFilter1D_dT_initialize'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT_initialize(void);
#endif
/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_dT_terminate.c
*
* Code generation for function 'positionKalmanFilter1D_dT_terminate'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D_dT.h"
#include "positionKalmanFilter1D_dT_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_dT_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_dT_terminate.h
*
* Code generation for function 'positionKalmanFilter1D_dT_terminate'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_dT_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_dT_terminate(void);
#endif
/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* positionKalmanFilter1D_dT_types.h
*
* Code generation for function 'positionKalmanFilter1D_dT'
*
* C source code generated on: Fri Nov 30 17:37:33 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (positionKalmanFilter1D_dT_types.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_initialize.c
*
* Code generation for function 'positionKalmanFilter1D_initialize'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
#include "positionKalmanFilter1D_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (positionKalmanFilter1D_initialize.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_initialize.h
*
* Code generation for function 'positionKalmanFilter1D_initialize'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_initialize(void);
#endif
/* End of code generation (positionKalmanFilter1D_initialize.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_terminate.c
*
* Code generation for function 'positionKalmanFilter1D_terminate'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "positionKalmanFilter1D.h"
#include "positionKalmanFilter1D_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void positionKalmanFilter1D_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (positionKalmanFilter1D_terminate.c) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
-31
View File
@@ -1,31 +0,0 @@
/*
* positionKalmanFilter1D_terminate.h
*
* Code generation for function 'positionKalmanFilter1D_terminate'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
#define __POSITIONKALMANFILTER1D_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include "rtwtypes.h"
#include "positionKalmanFilter1D_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void positionKalmanFilter1D_terminate(void);
#endif
/* End of code generation (positionKalmanFilter1D_terminate.h) */
src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
-16
View File
@@ -1,16 +0,0 @@
/*
* positionKalmanFilter1D_types.h
*
* Code generation for function 'positionKalmanFilter1D'
*
* C source code generated on: Fri Nov 30 14:26:11 2012
*
*/
#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
#define __POSITIONKALMANFILTER1D_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (positionKalmanFilter1D_types.h) */
src/modules/position_estimator_mc/codegen/randn.c
-524
View File
@@ -1,524 +0,0 @@
/*
* randn.c
*
* Code generation for function 'randn'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalman_dlqe3.h"
#include "randn.h"
#include "kalman_dlqe3_data.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
static uint32_T d_state[625];
/* Function Declarations */
static real_T b_genrandu(uint32_T mt[625]);
static real_T eml_rand_mt19937ar(uint32_T e_state[625]);
static real_T eml_rand_shr3cong(uint32_T e_state[2]);
static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);
static void twister_state_vector(uint32_T mt[625], real_T seed);
/* Function Definitions */
static real_T b_genrandu(uint32_T mt[625])
{
real_T r;
int32_T exitg1;
uint32_T u[2];
boolean_T isvalid;
int32_T k;
boolean_T exitg2;
/* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
/* <LEGAL> */
/* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
/* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
/* <LEGAL> */
/* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
/* <LEGAL> All rights reserved. */
/* <LEGAL> */
/* <LEGAL> Redistribution and use in source and binary forms, with or without */
/* <LEGAL> modification, are permitted provided that the following conditions */
/* <LEGAL> are met: */
/* <LEGAL> */
/* <LEGAL> 1. Redistributions of source code must retain the above copyright */
/* <LEGAL> notice, this list of conditions and the following disclaimer. */
/* <LEGAL> */
/* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
/* <LEGAL> notice, this list of conditions and the following disclaimer in the */
/* <LEGAL> documentation and/or other materials provided with the distribution. */
/* <LEGAL> */
/* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
/* <LEGAL> products derived from this software without specific prior written */
/* <LEGAL> permission. */
/* <LEGAL> */
/* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
/* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
/* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
/* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
/* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
/* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
/* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
/* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
/* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
/* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
/* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
do {
exitg1 = 0;
genrand_uint32_vector(mt, u);
r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
(u[1] >> 6U));
if (r == 0.0) {
if ((mt[624] >= 1U) && (mt[624] < 625U)) {
isvalid = TRUE;
} else {
isvalid = FALSE;
}
if (isvalid) {
isvalid = FALSE;
k = 1;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (k < 625)) {
if (mt[k - 1] == 0U) {
k++;
} else {
isvalid = TRUE;
exitg2 = TRUE;
}
}
}
if (!isvalid) {
twister_state_vector(mt, 5489.0);
}
} else {
exitg1 = 1;
}
} while (exitg1 == 0);
return r;
}
static real_T eml_rand_mt19937ar(uint32_T e_state[625])
{
real_T r;
int32_T exitg1;
uint32_T u32[2];
int32_T i;
static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,
0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
3.65415288536101, 3.91075795952492 };
real_T u;
static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,
0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
0.093365311912691, 0.0911136480663738, 0.0888735920682759,
0.0866451944505581, 0.0844285095703535, 0.082223595813203,
0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
0.000477467764609386 };
real_T x;
do {
exitg1 = 0;
genrand_uint32_vector(e_state, u32);
i = (int32_T)((u32[1] >> 24U) + 1U);
r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];
if (fabs(r) <= dv1[i - 1]) {
exitg1 = 1;
} else if (i < 256) {
u = b_genrandu(e_state);
if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {
exitg1 = 1;
}
} else {
do {
u = b_genrandu(e_state);
x = log(u) * 0.273661237329758;
u = b_genrandu(e_state);
} while (!(-2.0 * log(u) > x * x));
if (r < 0.0) {
r = x - 3.65415288536101;
} else {
r = 3.65415288536101 - x;
}
exitg1 = 1;
}
} while (exitg1 == 0);
return r;
}
static real_T eml_rand_shr3cong(uint32_T e_state[2])
{
real_T r;
uint32_T icng;
uint32_T jsr;
uint32_T ui;
int32_T j;
static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
real_T x;
real_T y;
real_T s;
icng = 69069U * e_state[0] + 1234567U;
jsr = e_state[1] ^ e_state[1] << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
ui = icng + jsr;
j = (int32_T)(ui & 63U);
r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];
if (fabs(r) <= dv0[j]) {
} else {
x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;
s = x + (0.5 + y);
if (s > 1.301198) {
if (r < 0.0) {
r = 0.4878992 * x - 0.4878992;
} else {
r = 0.4878992 - 0.4878992 * x;
}
} else if (s <= 0.9689279) {
} else {
s = 0.4878992 * x;
x = 0.4878992 - 0.4878992 * x;
if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {
if (r < 0.0) {
r = -(0.4878992 - s);
} else {
r = 0.4878992 - s;
}
} else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /
dv0[j + 1] <= exp(-0.5 * r * r)) {
} else {
do {
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /
2.776994;
icng = 69069U * icng + 1234567U;
jsr ^= jsr << 13U;
jsr ^= jsr >> 17U;
jsr ^= jsr << 5U;
} while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *
2.328306436538696E-10) > x * x));
if (r < 0.0) {
r = x - 2.776994;
} else {
r = 2.776994 - x;
}
}
}
}
e_state[0] = icng;
e_state[1] = jsr;
return r;
}
static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
{
int32_T i;
uint32_T mti;
int32_T kk;
uint32_T y;
uint32_T b_y;
uint32_T c_y;
uint32_T d_y;
for (i = 0; i < 2; i++) {
u[i] = 0U;
}
for (i = 0; i < 2; i++) {
mti = mt[624] + 1U;
if (mti >= 625U) {
for (kk = 0; kk < 227; kk++) {
y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
b_y = y >> 1U;
} else {
b_y = y >> 1U ^ 2567483615U;
}
mt[kk] = mt[397 + kk] ^ b_y;
}
for (kk = 0; kk < 396; kk++) {
y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
c_y = y >> 1U;
} else {
c_y = y >> 1U ^ 2567483615U;
}
mt[227 + kk] = mt[kk] ^ c_y;
}
y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
if ((int32_T)(y & 1U) == 0) {
d_y = y >> 1U;
} else {
d_y = y >> 1U ^ 2567483615U;
}
mt[623] = mt[396] ^ d_y;
mti = 1U;
}
y = mt[(int32_T)mti - 1];
mt[624] = mti;
y ^= y >> 11U;
y ^= y << 7U & 2636928640U;
y ^= y << 15U & 4022730752U;
y ^= y >> 18U;
u[i] = y;
}
}
static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)
{
int32_T hi;
uint32_T test1;
uint32_T test2;
hi = (int32_T)(s / 127773U);
test1 = 16807U * (s - (uint32_T)hi * 127773U);
test2 = 2836U * (uint32_T)hi;
if (test1 < test2) {
*e_state = (test1 - test2) + 2147483647U;
} else {
*e_state = test1 - test2;
}
*r = (real_T)*e_state * 4.6566128752457969E-10;
}
static void twister_state_vector(uint32_T mt[625], real_T seed)
{
uint32_T r;
int32_T mti;
if (seed < 4.294967296E+9) {
if (seed >= 0.0) {
r = (uint32_T)seed;
} else {
r = 0U;
}
} else if (seed >= 4.294967296E+9) {
r = MAX_uint32_T;
} else {
r = 0U;
}
mt[0] = r;
for (mti = 0; mti < 623; mti++) {
r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
mt[1 + mti] = r;
}
mt[624] = 624U;
}
real_T randn(void)
{
real_T r;
uint32_T e_state;
real_T t;
real_T b_r;
uint32_T f_state;
if (method == 0U) {
if (b_method == 4U) {
do {
genrandu(b_state, &e_state, &r);
genrandu(e_state, &b_state, &t);
b_r = 2.0 * r - 1.0;
t = 2.0 * t - 1.0;
t = t * t + b_r * b_r;
} while (!(t <= 1.0));
r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
} else if (b_method == 5U) {
r = eml_rand_shr3cong(c_state);
} else {
if (!state_not_empty) {
memset(&d_state[0], 0, 625U * sizeof(uint32_T));
twister_state_vector(d_state, 5489.0);
state_not_empty = TRUE;
}
r = eml_rand_mt19937ar(d_state);
}
} else if (method == 4U) {
e_state = state[0];
do {
genrandu(e_state, &f_state, &r);
genrandu(f_state, &e_state, &t);
b_r = 2.0 * r - 1.0;
t = 2.0 * t - 1.0;
t = t * t + b_r * b_r;
} while (!(t <= 1.0));
state[0] = e_state;
r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
} else {
r = eml_rand_shr3cong(state);
}
return r;
}
/* End of code generation (randn.c) */
src/modules/position_estimator_mc/codegen/randn.h
-33
View File
@@ -1,33 +0,0 @@
/*
* randn.h
*
* Code generation for function 'randn'
*
* C source code generated on: Tue Feb 19 15:26:32 2013
*
*/
#ifndef __RANDN_H__
#define __RANDN_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "kalman_dlqe3_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern real_T randn(void);
#endif
/* End of code generation (randn.h) */
src/modules/position_estimator_mc/codegen/rtGetInf.c
-139
View File
@@ -1,139 +0,0 @@
/*
* rtGetInf.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, Inf and MinusInf
*/
#include "rtGetInf.h"
#define NumBitsPerChar 8U
/* Function: rtGetInf ==================================================
* Abstract:
* Initialize rtInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T inf = 0.0;
if (bitsPerReal == 32U) {
inf = rtGetInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
inf = tmpVal.fltVal;
break;
}
}
}
return inf;
}
/* Function: rtGetInfF ==================================================
* Abstract:
* Initialize rtInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetInfF(void)
{
IEEESingle infF;
infF.wordL.wordLuint = 0x7F800000U;
return infF.wordL.wordLreal;
}
/* Function: rtGetMinusInf ==================================================
* Abstract:
* Initialize rtMinusInf needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetMinusInf(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T minf = 0.0;
if (bitsPerReal == 32U) {
minf = rtGetMinusInfF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF00000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
minf = tmpVal.fltVal;
break;
}
}
}
return minf;
}
/* Function: rtGetMinusInfF ==================================================
* Abstract:
* Initialize rtMinusInfF needed by the generated code.
* Inf is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetMinusInfF(void)
{
IEEESingle minfF;
minfF.wordL.wordLuint = 0xFF800000U;
return minfF.wordL.wordLreal;
}
/* End of code generation (rtGetInf.c) */
src/modules/position_estimator_mc/codegen/rtGetInf.h
-23
View File
@@ -1,23 +0,0 @@
/*
* rtGetInf.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RTGETINF_H__
#define __RTGETINF_H__
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetInf(void);
extern real32_T rtGetInfF(void);
extern real_T rtGetMinusInf(void);
extern real32_T rtGetMinusInfF(void);
#endif
/* End of code generation (rtGetInf.h) */
src/modules/position_estimator_mc/codegen/rtGetNaN.c
-96
View File
@@ -1,96 +0,0 @@
/*
* rtGetNaN.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finite, NaN
*/
#include "rtGetNaN.h"
#define NumBitsPerChar 8U
/* Function: rtGetNaN ==================================================
* Abstract:
* Initialize rtNaN needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real_T rtGetNaN(void)
{
size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
real_T nan = 0.0;
if (bitsPerReal == 32U) {
nan = rtGetNaNF();
} else {
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
union {
LittleEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0xFFF80000U;
tmpVal.bitVal.words.wordL = 0x00000000U;
nan = tmpVal.fltVal;
break;
}
case BigEndian:
{
union {
BigEndianIEEEDouble bitVal;
real_T fltVal;
} tmpVal;
tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
nan = tmpVal.fltVal;
break;
}
}
}
return nan;
}
/* Function: rtGetNaNF ==================================================
* Abstract:
* Initialize rtNaNF needed by the generated code.
* NaN is initialized as non-signaling. Assumes IEEE.
*/
real32_T rtGetNaNF(void)
{
IEEESingle nanF = { { 0 } };
uint16_T one = 1U;
enum {
LittleEndian,
BigEndian
} machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
switch (machByteOrder) {
case LittleEndian:
{
nanF.wordL.wordLuint = 0xFFC00000U;
break;
}
case BigEndian:
{
nanF.wordL.wordLuint = 0x7FFFFFFFU;
break;
}
}
return nanF.wordL.wordLreal;
}
/* End of code generation (rtGetNaN.c) */
src/modules/position_estimator_mc/codegen/rtGetNaN.h
-21
View File
@@ -1,21 +0,0 @@
/*
* rtGetNaN.h
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
#ifndef __RTGETNAN_H__
#define __RTGETNAN_H__
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#endif
/* End of code generation (rtGetNaN.h) */
src/modules/position_estimator_mc/codegen/rt_nonfinite.c
-87
View File
@@ -1,87 +0,0 @@
/*
* rt_nonfinite.c
*
* Code generation for function 'kalman_dlqe2'
*
* C source code generated on: Thu Feb 14 12:52:29 2013
*
*/
/*
* Abstract:
* MATLAB for code generation function to initialize non-finites,
* (Inf, NaN and -Inf).
*/
#include "rt_nonfinite.h"
#include "rtGetNaN.h"
#include "rtGetInf.h"
real_T rtInf;
real_T rtMinusInf;
real_T rtNaN;
real32_T rtInfF;
real32_T rtMinusInfF;
real32_T rtNaNF;
/* Function: rt_InitInfAndNaN ==================================================
* Abstract:
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
*/
void rt_InitInfAndNaN(size_t realSize)
{
(void) (realSize);
rtNaN = rtGetNaN();
rtNaNF = rtGetNaNF();
rtInf = rtGetInf();
rtInfF = rtGetInfF();
rtMinusInf = rtGetMinusInf();
rtMinusInfF = rtGetMinusInfF();
}
/* Function: rtIsInf ==================================================
* Abstract:
* Test if value is infinite
*/
boolean_T rtIsInf(real_T value)
{
return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
}
/* Function: rtIsInfF =================================================
* Abstract:
* Test if single-precision value is infinite
*/
boolean_T rtIsInfF(real32_T value)
{
return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
}
/* Function: rtIsNaN ==================================================
* Abstract:
* Test if value is not a number
*/
boolean_T rtIsNaN(real_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan(value)? TRUE:FALSE;
#else
return (value!=value)? 1U:0U;
#endif
}
/* Function: rtIsNaNF =================================================
* Abstract:
* Test if single-precision value is not a number
*/
boolean_T rtIsNaNF(real32_T value)
{
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
return _isnan((real_T)value)? true:false;
#else
return (value!=value)? 1U:0U;
#endif
}
/* End of code generation (rt_nonfinite.c) */

Some files were not shown because too many files have changed in this diff Show More