forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
commit
9f2d65eff5
|
@ -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;
|
||||
|
|
|
@ -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];
|
||||
|
@ -149,7 +155,7 @@ private:
|
|||
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,7 +548,11 @@ 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;
|
||||
|
@ -523,7 +560,7 @@ PX4FMU::task_main()
|
|||
|
||||
/* sleep waiting for data, stopping to check for PPM
|
||||
* input at 100Hz */
|
||||
int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
|
||||
int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
|
||||
|
||||
/* this would be bad... */
|
||||
if (ret < 0) {
|
||||
|
@ -537,89 +574,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 +707,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 +735,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 +1104,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
if (_mixers != nullptr) {
|
||||
delete _mixers;
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
}
|
||||
|
||||
break;
|
||||
|
@ -1061,18 +1113,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 +1137,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 +1151,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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -1285,6 +1285,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (res == TRANSITION_DENIED) {
|
||||
/* LAND not allowed, set TERMINATION state */
|
||||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
|
||||
}
|
||||
|
||||
} else if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1295,15 +1302,25 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (!status.condition_landed) {
|
||||
/* vehicle is not landed, try to perform RTL */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND");
|
||||
}
|
||||
}
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
|
||||
}
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
/* LAND not allowed, set TERMINATION state */
|
||||
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
|
||||
} else if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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_ */
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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_ */
|
|
@ -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
|
|
@ -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(¶m_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);
|
||||
}
|
|
@ -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
|
|
@ -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
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -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) */
|
|
@ -1,53 +0,0 @@
|
|||
/*
|
||||
* rt_nonfinite.h
|
||||
*
|
||||
* Code generation for function 'kalman_dlqe2'
|
||||
*
|
||||
* C source code generated on: Thu Feb 14 12:52:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RT_NONFINITE_H__
|
||||
#define __RT_NONFINITE_H__
|
||||
|
||||
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
|
||||
#include <float.h>
|
||||
#endif
|
||||
#include <stddef.h>
|
||||
#include "rtwtypes.h"
|
||||
|
||||
extern real_T rtInf;
|
||||
extern real_T rtMinusInf;
|
||||
extern real_T rtNaN;
|
||||
extern real32_T rtInfF;
|
||||
extern real32_T rtMinusInfF;
|
||||
extern real32_T rtNaNF;
|
||||
extern void rt_InitInfAndNaN(size_t realSize);
|
||||
extern boolean_T rtIsInf(real_T value);
|
||||
extern boolean_T rtIsInfF(real32_T value);
|
||||
extern boolean_T rtIsNaN(real_T value);
|
||||
extern boolean_T rtIsNaNF(real32_T value);
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordH;
|
||||
uint32_T wordL;
|
||||
} words;
|
||||
} BigEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
uint32_T wordL;
|
||||
uint32_T wordH;
|
||||
} words;
|
||||
} LittleEndianIEEEDouble;
|
||||
|
||||
typedef struct {
|
||||
union {
|
||||
real32_T wordLreal;
|
||||
uint32_T wordLuint;
|
||||
} wordL;
|
||||
} IEEESingle;
|
||||
|
||||
#endif
|
||||
/* End of code generation (rt_nonfinite.h) */
|
|
@ -1,159 +0,0 @@
|
|||
/*
|
||||
* rtwtypes.h
|
||||
*
|
||||
* Code generation for function 'kalman_dlqe2'
|
||||
*
|
||||
* C source code generated on: Thu Feb 14 12:52:29 2013
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __RTWTYPES_H__
|
||||
#define __RTWTYPES_H__
|
||||
#ifndef TRUE
|
||||
# define TRUE (1U)
|
||||
#endif
|
||||
#ifndef FALSE
|
||||
# define FALSE (0U)
|
||||
#endif
|
||||
#ifndef __TMWTYPES__
|
||||
#define __TMWTYPES__
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
/*=======================================================================*
|
||||
* Target hardware information
|
||||
* Device type: Generic->MATLAB Host Computer
|
||||
* Number of bits: char: 8 short: 16 int: 32
|
||||
* long: 32 native word size: 32
|
||||
* Byte ordering: LittleEndian
|
||||
* Signed integer division rounds to: Undefined
|
||||
* Shift right on a signed integer as arithmetic shift: off
|
||||
*=======================================================================*/
|
||||
|
||||
/*=======================================================================*
|
||||
* Fixed width word size data types: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
* real32_T, real64_T - 32 and 64 bit floating point numbers *
|
||||
*=======================================================================*/
|
||||
|
||||
typedef signed char int8_T;
|
||||
typedef unsigned char uint8_T;
|
||||
typedef short int16_T;
|
||||
typedef unsigned short uint16_T;
|
||||
typedef int int32_T;
|
||||
typedef unsigned int uint32_T;
|
||||
typedef float real32_T;
|
||||
typedef double real64_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
|
||||
* ulong_T, char_T and byte_T. *
|
||||
*===========================================================================*/
|
||||
|
||||
typedef double real_T;
|
||||
typedef double time_T;
|
||||
typedef unsigned char boolean_T;
|
||||
typedef int int_T;
|
||||
typedef unsigned int uint_T;
|
||||
typedef unsigned long ulong_T;
|
||||
typedef char char_T;
|
||||
typedef char_T byte_T;
|
||||
|
||||
/*===========================================================================*
|
||||
* Complex number type definitions *
|
||||
*===========================================================================*/
|
||||
#define CREAL_T
|
||||
typedef struct {
|
||||
real32_T re;
|
||||
real32_T im;
|
||||
} creal32_T;
|
||||
|
||||
typedef struct {
|
||||
real64_T re;
|
||||
real64_T im;
|
||||
} creal64_T;
|
||||
|
||||
typedef struct {
|
||||
real_T re;
|
||||
real_T im;
|
||||
} creal_T;
|
||||
|
||||
typedef struct {
|
||||
int8_T re;
|
||||
int8_T im;
|
||||
} cint8_T;
|
||||
|
||||
typedef struct {
|
||||
uint8_T re;
|
||||
uint8_T im;
|
||||
} cuint8_T;
|
||||
|
||||
typedef struct {
|
||||
int16_T re;
|
||||
int16_T im;
|
||||
} cint16_T;
|
||||
|
||||
typedef struct {
|
||||
uint16_T re;
|
||||
uint16_T im;
|
||||
} cuint16_T;
|
||||
|
||||
typedef struct {
|
||||
int32_T re;
|
||||
int32_T im;
|
||||
} cint32_T;
|
||||
|
||||
typedef struct {
|
||||
uint32_T re;
|
||||
uint32_T im;
|
||||
} cuint32_T;
|
||||
|
||||
|
||||
/*=======================================================================*
|
||||
* Min and Max: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
*=======================================================================*/
|
||||
|
||||
#define MAX_int8_T ((int8_T)(127))
|
||||
#define MIN_int8_T ((int8_T)(-128))
|
||||
#define MAX_uint8_T ((uint8_T)(255))
|
||||
#define MIN_uint8_T ((uint8_T)(0))
|
||||
#define MAX_int16_T ((int16_T)(32767))
|
||||
#define MIN_int16_T ((int16_T)(-32768))
|
||||
#define MAX_uint16_T ((uint16_T)(65535))
|
||||
#define MIN_uint16_T ((uint16_T)(0))
|
||||
#define MAX_int32_T ((int32_T)(2147483647))
|
||||
#define MIN_int32_T ((int32_T)(-2147483647-1))
|
||||
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
|
||||
#define MIN_uint32_T ((uint32_T)(0))
|
||||
|
||||
/* Logical type definitions */
|
||||
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
|
||||
# ifndef false
|
||||
# define false (0U)
|
||||
# endif
|
||||
# ifndef true
|
||||
# define true (1U)
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/*
|
||||
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
|
||||
* for signed integer values.
|
||||
*/
|
||||
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
|
||||
#error "This code must be compiled using a 2's complement representation for signed integer values"
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Maximum length of a MATLAB identifier (function/variable)
|
||||
* including the null-termination character. Referenced by
|
||||
* rt_logging.c and rt_matrx.c.
|
||||
*/
|
||||
#define TMW_NAME_LENGTH_MAX 64
|
||||
|
||||
#endif
|
||||
#endif
|
||||
/* End of code generation (rtwtypes.h) */
|
|
@ -1,3 +0,0 @@
|
|||
function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
|
||||
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
|
||||
end
|
|
@ -1,9 +0,0 @@
|
|||
function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
|
||||
st = 1/2*dt^2;
|
||||
A = [1,dt,st;
|
||||
0,1,dt;
|
||||
0,0,1];
|
||||
C=[1,0,0];
|
||||
K = [k1;k2;k3];
|
||||
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
|
||||
end
|
|
@ -1,17 +0,0 @@
|
|||
function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
|
||||
st = 1/2*dt^2;
|
||||
A = [1,dt,st;
|
||||
0,1,dt;
|
||||
0,0,1];
|
||||
C=[1,0,0];
|
||||
K = [k1;k2;k3];
|
||||
if addNoise==1
|
||||
noise = sigma*randn(1,1);
|
||||
z = z + noise;
|
||||
end
|
||||
if(posUpdate)
|
||||
x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
|
||||
else
|
||||
x_aposteriori=A*x_aposteriori_k;
|
||||
end
|
||||
end
|
|
@ -1,60 +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_mc
|
||||
|
||||
SRCS = position_estimator_mc_main.c \
|
||||
position_estimator_mc_params.c \
|
||||
codegen/positionKalmanFilter1D_initialize.c \
|
||||
codegen/positionKalmanFilter1D_terminate.c \
|
||||
codegen/positionKalmanFilter1D.c \
|
||||
codegen/rt_nonfinite.c \
|
||||
codegen/rtGetInf.c \
|
||||
codegen/rtGetNaN.c \
|
||||
codegen/positionKalmanFilter1D_dT_initialize.c \
|
||||
codegen/positionKalmanFilter1D_dT_terminate.c \
|
||||
codegen/kalman_dlqe1.c \
|
||||
codegen/kalman_dlqe1_initialize.c \
|
||||
codegen/kalman_dlqe1_terminate.c \
|
||||
codegen/kalman_dlqe2.c \
|
||||
codegen/kalman_dlqe2_initialize.c \
|
||||
codegen/kalman_dlqe2_terminate.c \
|
||||
codegen/kalman_dlqe3.c \
|
||||
codegen/kalman_dlqe3_initialize.c \
|
||||
codegen/kalman_dlqe3_terminate.c \
|
||||
codegen/kalman_dlqe3_data.c \
|
||||
codegen/randn.c
|
|
@ -1,19 +0,0 @@
|
|||
function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
|
||||
%prediction
|
||||
x_apriori=A*x_aposteriori_k+B*u;
|
||||
P_apriori=A*P_aposteriori_k*A'+Q;
|
||||
if abs(u)<thresh
|
||||
x_apriori(2)=decay*x_apriori(2);
|
||||
end
|
||||
%update
|
||||
if gps_update==1
|
||||
y=z-C*x_apriori;
|
||||
S=C*P_apriori*C'+R;
|
||||
K=(P_apriori*C')/S;
|
||||
x_aposteriori=x_apriori+K*y;
|
||||
P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
|
||||
else
|
||||
x_aposteriori=x_apriori;
|
||||
P_aposteriori=P_apriori;
|
||||
end
|
||||
end
|
|
@ -1,26 +0,0 @@
|
|||
function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
|
||||
%dynamics
|
||||
A = [1 dT -0.5*dT*dT;
|
||||
0 1 -dT;
|
||||
0 0 1];
|
||||
B = [0.5*dT*dT; dT; 0];
|
||||
C = [1 0 0];
|
||||
%prediction
|
||||
x_apriori=A*x_aposteriori_k+B*u;
|
||||
P_apriori=A*P_aposteriori_k*A'+Q;
|
||||
if abs(u)<thresh
|
||||
x_apriori(2)=decay*x_apriori(2);
|
||||
end
|
||||
%update
|
||||
if gps_update==1
|
||||
y=z-C*x_apriori;
|
||||
S=C*P_apriori*C'+R;
|
||||
K=(P_apriori*C')/S;
|
||||
x_aposteriori=x_apriori+K*y;
|
||||
P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
|
||||
else
|
||||
x_aposteriori=x_apriori;
|
||||
P_aposteriori=P_apriori;
|
||||
end
|
||||
end
|
||||
|
|
@ -1,515 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Damian Aregger <daregger@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@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 <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <float.h>
|
||||
#include <string.h>
|
||||
#include <nuttx/config.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/sensor_combined.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <poll.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "position_estimator_mc_params.h"
|
||||
//#include <uORB/topics/debug_key_value.h>
|
||||
#include "codegen/kalman_dlqe2.h"
|
||||
#include "codegen/kalman_dlqe2_initialize.h"
|
||||
#include "codegen/kalman_dlqe3.h"
|
||||
#include "codegen/kalman_dlqe3_initialize.h"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int position_estimator_mc_task; /**< Handle of deamon task / thread */
|
||||
|
||||
__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
|
||||
|
||||
int position_estimator_mc_thread_main(int argc, char *argv[]);
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
warnx("%s\n", reason);
|
||||
warnx("usage: position_estimator_mc {start|stop|status}");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The position_estimator_mc_thread 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 position_estimator_mc_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("position_estimator_mc already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
position_estimator_mc_task = task_spawn_cmd("position_estimator_mc",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4096,
|
||||
position_estimator_mc_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
warnx("position_estimator_mc is running");
|
||||
} else {
|
||||
warnx("position_estimator_mc not started");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* main
|
||||
****************************************************************************/
|
||||
int position_estimator_mc_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* welcome user */
|
||||
warnx("[position_estimator_mc] started");
|
||||
int mavlink_fd;
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
|
||||
|
||||
/* initialize values */
|
||||
float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
|
||||
// float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f};
|
||||
float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
|
||||
float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
|
||||
float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
|
||||
float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
|
||||
float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
|
||||
float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
|
||||
|
||||
// XXX this is terribly wrong and should actual dT instead
|
||||
const float dT_const_50 = 1.0f/50.0f;
|
||||
|
||||
float addNoise = 0.0f;
|
||||
float sigma = 0.0f;
|
||||
//computed from dlqe in matlab
|
||||
const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
|
||||
// XXX implement baro filter
|
||||
const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
|
||||
float K[3] = {0.0f, 0.0f, 0.0f};
|
||||
int baro_loop_cnt = 0;
|
||||
int baro_loop_end = 70; /* measurement for 1 second */
|
||||
float p0_Pa = 0.0f; /* to determin while start up */
|
||||
float rho0 = 1.293f; /* standard pressure */
|
||||
const float const_earth_gravity = 9.81f;
|
||||
|
||||
float posX = 0.0f;
|
||||
float posY = 0.0f;
|
||||
float posZ = 0.0f;
|
||||
|
||||
double lat_current;
|
||||
double lon_current;
|
||||
float alt_current;
|
||||
|
||||
float gps_origin_altitude = 0.0f;
|
||||
|
||||
/* Initialize filter */
|
||||
kalman_dlqe2_initialize();
|
||||
kalman_dlqe3_initialize();
|
||||
|
||||
/* declare and safely initialize all structs */
|
||||
struct sensor_combined_s sensor;
|
||||
memset(&sensor, 0, sizeof(sensor));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att, 0, sizeof(att));
|
||||
struct vehicle_status_s vehicle_status;
|
||||
memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
|
||||
struct vehicle_vicon_position_s vicon_pos;
|
||||
memset(&vicon_pos, 0, sizeof(vicon_pos));
|
||||
struct actuator_controls_effective_s act_eff;
|
||||
memset(&act_eff, 0, sizeof(act_eff));
|
||||
struct vehicle_gps_position_s gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
struct vehicle_local_position_s local_pos_est;
|
||||
memset(&local_pos_est, 0, sizeof(local_pos_est));
|
||||
struct vehicle_global_position_s global_pos_est;
|
||||
memset(&global_pos_est, 0, sizeof(global_pos_est));
|
||||
|
||||
/* subscribe */
|
||||
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
int sub_params = orb_subscribe(ORB_ID(parameter_update));
|
||||
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
|
||||
int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* advertise */
|
||||
orb_advert_t local_pos_est_pub = 0;
|
||||
orb_advert_t global_pos_est_pub = 0;
|
||||
|
||||
struct position_estimator_mc_params pos1D_params;
|
||||
struct position_estimator_mc_param_handles pos1D_param_handles;
|
||||
/* initialize parameter handles */
|
||||
parameters_init(&pos1D_param_handles);
|
||||
|
||||
bool flag_use_gps = false;
|
||||
bool flag_use_baro = false;
|
||||
bool flag_baro_initialized = false; /* in any case disable baroINITdone */
|
||||
/* FIRST PARAMETER READ at START UP*/
|
||||
struct parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
|
||||
/* FIRST PARAMETER UPDATE */
|
||||
parameters_update(&pos1D_param_handles, &pos1D_params);
|
||||
flag_use_baro = pos1D_params.baro;
|
||||
sigma = pos1D_params.sigma;
|
||||
addNoise = pos1D_params.addNoise;
|
||||
/* END FIRST PARAMETER UPDATE */
|
||||
|
||||
/* try to grab a vicon message - if it fails, go for GPS. */
|
||||
|
||||
/* make sure the next orb_check() can't return true unless we get a timely update */
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
|
||||
/* allow 200 ms for vicon to come in */
|
||||
usleep(200000);
|
||||
/* check if we got vicon */
|
||||
bool update_check;
|
||||
orb_check(vicon_pos_sub, &update_check);
|
||||
/* if no update was available, use GPS */
|
||||
flag_use_gps = !update_check;
|
||||
|
||||
if (flag_use_gps) {
|
||||
mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
|
||||
/* wait until gps signal turns valid, only then can we initialize the projection */
|
||||
|
||||
// XXX magic number
|
||||
float hdop_threshold_m = 4.0f;
|
||||
float vdop_threshold_m = 8.0f;
|
||||
|
||||
/*
|
||||
* If horizontal dilution of precision (hdop / eph)
|
||||
* and vertical diluation of precision (vdop / epv)
|
||||
* are below a certain threshold (e.g. 4 m), AND
|
||||
* home position is not yet set AND the last GPS
|
||||
* GPS measurement is not older than two seconds AND
|
||||
* the system is currently not armed, set home
|
||||
* position to the current position.
|
||||
*/
|
||||
|
||||
while (!(gps.fix_type == 3
|
||||
&& (gps.eph_m < hdop_threshold_m)
|
||||
&& (gps.epv_m < vdop_threshold_m)
|
||||
&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
|
||||
|
||||
struct pollfd fds1[2] = {
|
||||
{ .fd = vehicle_gps_sub, .events = POLLIN },
|
||||
{ .fd = sub_params, .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(fds1, 2, 5000)) {
|
||||
if (fds1[0].revents & POLLIN){
|
||||
/* Read gps position */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
|
||||
}
|
||||
if (fds1[1].revents & POLLIN){
|
||||
/* Read out parameters to check for an update there, e.g. useGPS variable */
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s updated;
|
||||
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
|
||||
/* update parameters */
|
||||
parameters_update(&pos1D_param_handles, &pos1D_params);
|
||||
}
|
||||
}
|
||||
static int printcounter = 0;
|
||||
if (printcounter == 100) {
|
||||
printcounter = 0;
|
||||
warnx("[pos_est_mc] wait for GPS fix");
|
||||
}
|
||||
printcounter++;
|
||||
}
|
||||
|
||||
/* get gps value for first initialization */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
|
||||
lat_current = ((double)(gps.lat)) * 1e-7d;
|
||||
lon_current = ((double)(gps.lon)) * 1e-7d;
|
||||
alt_current = gps.alt * 1e-3f;
|
||||
gps_origin_altitude = alt_current;
|
||||
/* initialize coordinates */
|
||||
map_projection_init(lat_current, lon_current);
|
||||
/* publish global position messages only after first GPS message */
|
||||
printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
|
||||
/* onboard calculated position estimations */
|
||||
}
|
||||
thread_running = true;
|
||||
|
||||
struct pollfd fds2[3] = {
|
||||
{ .fd = vehicle_gps_sub, .events = POLLIN },
|
||||
{ .fd = vicon_pos_sub, .events = POLLIN },
|
||||
{ .fd = sub_params, .events = POLLIN },
|
||||
};
|
||||
|
||||
bool vicon_updated = false;
|
||||
bool gps_updated = false;
|
||||
|
||||
/**< main_loop */
|
||||
while (!thread_should_exit) {
|
||||
int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
|
||||
if (ret < 0) {
|
||||
/* poll error */
|
||||
} else {
|
||||
if (fds2[2].revents & POLLIN){
|
||||
/* new parameter */
|
||||
/* read from param to clear updated flag */
|
||||
struct parameter_update_s updated;
|
||||
orb_copy(ORB_ID(parameter_update), sub_params, &updated);
|
||||
/* update parameters */
|
||||
parameters_update(&pos1D_param_handles, &pos1D_params);
|
||||
flag_use_baro = pos1D_params.baro;
|
||||
sigma = pos1D_params.sigma;
|
||||
addNoise = pos1D_params.addNoise;
|
||||
}
|
||||
vicon_updated = false; /* default is no vicon_updated */
|
||||
if (fds2[1].revents & POLLIN) {
|
||||
/* new vicon position */
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
|
||||
posX = vicon_pos.x;
|
||||
posY = vicon_pos.y;
|
||||
posZ = vicon_pos.z;
|
||||
vicon_updated = true; /* set flag for vicon update */
|
||||
} /* end of poll call for vicon updates */
|
||||
gps_updated = false;
|
||||
if (fds2[0].revents & POLLIN) {
|
||||
/* new GPS value */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
|
||||
/* Project gps lat lon (Geographic coordinate system) to plane*/
|
||||
map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
|
||||
posX = z[0];
|
||||
posY = z[1];
|
||||
posZ = (float)(gps.alt * 1e-3f);
|
||||
gps_updated = true;
|
||||
}
|
||||
|
||||
/* Main estimator loop */
|
||||
orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
|
||||
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
|
||||
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
|
||||
// barometric pressure estimation at start up
|
||||
if (!flag_baro_initialized){
|
||||
// mean calculation over several measurements
|
||||
if (baro_loop_cnt<baro_loop_end) {
|
||||
p0_Pa += (sensor.baro_pres_mbar*100);
|
||||
baro_loop_cnt++;
|
||||
} else {
|
||||
p0_Pa /= (float)(baro_loop_cnt);
|
||||
flag_baro_initialized = true;
|
||||
char *baro_m_start = "barometer initialized with p0 = ";
|
||||
char p0_char[15];
|
||||
sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
|
||||
char *baro_m_end = " mbar";
|
||||
char str[80];
|
||||
strcpy(str,baro_m_start);
|
||||
strcat(str,p0_char);
|
||||
strcat(str,baro_m_end);
|
||||
mavlink_log_info(mavlink_fd, str);
|
||||
}
|
||||
}
|
||||
if (flag_use_gps) {
|
||||
/* initialize map projection with the last estimate (not at full rate) */
|
||||
if (gps.fix_type > 2) {
|
||||
/* x-y-position/velocity estimation in earth frame = gps frame */
|
||||
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
|
||||
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
|
||||
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
|
||||
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
|
||||
/* z-position/velocity estimation in earth frame = vicon frame */
|
||||
float z_est = 0.0f;
|
||||
if (flag_baro_initialized && flag_use_baro) {
|
||||
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
|
||||
K[0] = K_vicon_50Hz[0];
|
||||
K[1] = K_vicon_50Hz[1];
|
||||
K[2] = K_vicon_50Hz[2];
|
||||
gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
|
||||
} else {
|
||||
z_est = posZ;
|
||||
K[0] = K_vicon_50Hz[0];
|
||||
K[1] = K_vicon_50Hz[1];
|
||||
K[2] = K_vicon_50Hz[2];
|
||||
}
|
||||
|
||||
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
|
||||
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
|
||||
local_pos_est.x = x_x_aposteriori_k[0];
|
||||
local_pos_est.vx = x_x_aposteriori_k[1];
|
||||
local_pos_est.y = x_y_aposteriori_k[0];
|
||||
local_pos_est.vy = x_y_aposteriori_k[1];
|
||||
local_pos_est.z = x_z_aposteriori_k[0];
|
||||
local_pos_est.vz = x_z_aposteriori_k[1];
|
||||
local_pos_est.timestamp = hrt_absolute_time();
|
||||
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
|
||||
/* publish local position estimate */
|
||||
if (local_pos_est_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
|
||||
} else {
|
||||
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
|
||||
}
|
||||
/* publish on GPS updates */
|
||||
if (gps_updated) {
|
||||
|
||||
double lat, lon;
|
||||
float alt = z_est + gps_origin_altitude;
|
||||
|
||||
map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
|
||||
|
||||
global_pos_est.lat = lat;
|
||||
global_pos_est.lon = lon;
|
||||
global_pos_est.alt = alt;
|
||||
|
||||
if (global_pos_est_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
|
||||
} else {
|
||||
global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/* x-y-position/velocity estimation in earth frame = vicon frame */
|
||||
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
|
||||
memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
|
||||
kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
|
||||
memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
|
||||
/* z-position/velocity estimation in earth frame = vicon frame */
|
||||
float z_est = 0.0f;
|
||||
float local_sigma = 0.0f;
|
||||
if (flag_baro_initialized && flag_use_baro) {
|
||||
z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
|
||||
K[0] = K_vicon_50Hz[0];
|
||||
K[1] = K_vicon_50Hz[1];
|
||||
K[2] = K_vicon_50Hz[2];
|
||||
vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
|
||||
local_sigma = 0.0f; /* don't add noise on barometer in any case */
|
||||
} else {
|
||||
z_est = posZ;
|
||||
K[0] = K_vicon_50Hz[0];
|
||||
K[1] = K_vicon_50Hz[1];
|
||||
K[2] = K_vicon_50Hz[2];
|
||||
local_sigma = sigma;
|
||||
}
|
||||
kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
|
||||
memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
|
||||
local_pos_est.x = x_x_aposteriori_k[0];
|
||||
local_pos_est.vx = x_x_aposteriori_k[1];
|
||||
local_pos_est.y = x_y_aposteriori_k[0];
|
||||
local_pos_est.vy = x_y_aposteriori_k[1];
|
||||
local_pos_est.z = x_z_aposteriori_k[0];
|
||||
local_pos_est.vz = x_z_aposteriori_k[1];
|
||||
local_pos_est.timestamp = hrt_absolute_time();
|
||||
if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
|
||||
if(local_pos_est_pub > 0)
|
||||
orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
|
||||
else
|
||||
local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
|
||||
//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
|
||||
//mavlink_log_info(mavlink_fd, buf);
|
||||
}
|
||||
}
|
||||
} /* end of poll return value check */
|
||||
}
|
||||
|
||||
printf("[pos_est_mc] exiting.\n");
|
||||
mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
|
@ -1,68 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Damian Aregger <daregger@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@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_mc_params.c
|
||||
*
|
||||
* Parameters for position_estimator_mc
|
||||
*/
|
||||
|
||||
#include "position_estimator_mc_params.h"
|
||||
|
||||
/* Kalman Filter covariances */
|
||||
/* gps measurement noise standard deviation */
|
||||
PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
|
||||
PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
|
||||
|
||||
int parameters_init(struct position_estimator_mc_param_handles *h)
|
||||
{
|
||||
h->addNoise = param_find("POS_EST_ADDN");
|
||||
h->sigma = param_find("POS_EST_SIGMA");
|
||||
h->r = param_find("POS_EST_R");
|
||||
h->baro_param_handle = param_find("POS_EST_BARO");
|
||||
return OK;
|
||||
}
|
||||
|
||||
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
|
||||
{
|
||||
param_get(h->addNoise, &(p->addNoise));
|
||||
param_get(h->sigma, &(p->sigma));
|
||||
param_get(h->r, &(p->R));
|
||||
param_get(h->baro_param_handle, &(p->baro));
|
||||
return OK;
|
||||
}
|
|
@ -1,69 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Damian Aregger <daregger@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@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_mc_params.h
|
||||
*
|
||||
* Parameters for Position Estimator
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct position_estimator_mc_params {
|
||||
float addNoise;
|
||||
float sigma;
|
||||
float R;
|
||||
int baro; /* consider barometer */
|
||||
};
|
||||
|
||||
struct position_estimator_mc_param_handles {
|
||||
param_t addNoise;
|
||||
param_t sigma;
|
||||
param_t r;
|
||||
param_t baro_param_handle;
|
||||
};
|
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values
|
||||
*
|
||||
*/
|
||||
int parameters_init(struct position_estimator_mc_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
|
|
@ -1,43 +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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# sdlog Application
|
||||
#
|
||||
|
||||
MODULE_COMMAND = sdlog
|
||||
# The main thread only buffers to RAM, needs a high priority
|
||||
MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
|
||||
|
||||
SRCS = sdlog.c \
|
||||
sdlog_ringbuffer.c
|
|
@ -1,840 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: 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 sdlog.c
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Simple SD logger for flight data. Buffers new sensor values and
|
||||
* does the heavy SD I/O in a low-priority worker thread.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <unistd.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "sdlog_ringbuffer.h"
|
||||
|
||||
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 */
|
||||
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
|
||||
|
||||
static const char *mountpoint = "/fs/microsd";
|
||||
static const char *mfile_in = "/etc/logging/logconv.m";
|
||||
int sysvector_file = -1;
|
||||
int mavlink_fd = -1;
|
||||
struct sdlog_logbuffer lb;
|
||||
|
||||
/* mutex / condition to synchronize threads */
|
||||
pthread_mutex_t sysvector_mutex;
|
||||
pthread_cond_t sysvector_cond;
|
||||
|
||||
/**
|
||||
* System state vector log buffer writing
|
||||
*/
|
||||
static void *sdlog_sysvector_write_thread(void *arg);
|
||||
|
||||
/**
|
||||
* Create the thread to write the system vector
|
||||
*/
|
||||
pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf);
|
||||
|
||||
/**
|
||||
* SD log management function.
|
||||
*/
|
||||
__EXPORT int sdlog_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of sd log deamon.
|
||||
*/
|
||||
int sdlog_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static int file_exist(const char *filename);
|
||||
|
||||
static int file_copy(const char *file_old, const char *file_new);
|
||||
|
||||
static void handle_command(struct vehicle_command_s *cmd);
|
||||
|
||||
/**
|
||||
* Print the current status.
|
||||
*/
|
||||
static void print_sdlog_status(void);
|
||||
|
||||
/**
|
||||
* Create folder for current logging session.
|
||||
*/
|
||||
static int create_logfolder(char *folder_path);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
|
||||
}
|
||||
|
||||
// XXX turn this into a C++ class
|
||||
unsigned sensor_combined_bytes = 0;
|
||||
unsigned actuator_outputs_bytes = 0;
|
||||
unsigned actuator_controls_bytes = 0;
|
||||
unsigned sysvector_bytes = 0;
|
||||
unsigned blackbox_file_bytes = 0;
|
||||
uint64_t starttime = 0;
|
||||
|
||||
/* logging on or off, default to true */
|
||||
bool logging_enabled = true;
|
||||
|
||||
/**
|
||||
* The sd log 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_spawn_cmd().
|
||||
*/
|
||||
int sdlog_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("sdlog already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd("sdlog",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT - 30,
|
||||
4096,
|
||||
sdlog_thread_main,
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (!thread_running) {
|
||||
printf("\tsdlog is not started\n");
|
||||
}
|
||||
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
print_sdlog_status();
|
||||
|
||||
} else {
|
||||
printf("\tsdlog not started\n");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int create_logfolder(char *folder_path)
|
||||
{
|
||||
/* make folder on sdcard */
|
||||
uint16_t foldernumber = 1; // start with folder 0001
|
||||
int mkdir_ret;
|
||||
|
||||
/* look for the next folder that does not exist */
|
||||
while (foldernumber < MAX_NO_LOGFOLDER) {
|
||||
/* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */
|
||||
sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber);
|
||||
mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
/* the result is -1 if the folder exists */
|
||||
|
||||
if (mkdir_ret == 0) {
|
||||
/* folder does not exist, success */
|
||||
|
||||
/* now copy the Matlab/Octave file */
|
||||
char mfile_out[100];
|
||||
sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
|
||||
int ret = file_copy(mfile_in, mfile_out);
|
||||
|
||||
if (!ret) {
|
||||
warnx("copied m file to %s", mfile_out);
|
||||
|
||||
} else {
|
||||
warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
} else if (mkdir_ret == -1) {
|
||||
/* folder exists already */
|
||||
foldernumber++;
|
||||
continue;
|
||||
|
||||
} else {
|
||||
warn("failed creating new folder");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (foldernumber >= MAX_NO_LOGFOLDER) {
|
||||
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
|
||||
warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void *
|
||||
sdlog_sysvector_write_thread(void *arg)
|
||||
{
|
||||
/* set name */
|
||||
prctl(PR_SET_NAME, "sdlog microSD I/O", 0);
|
||||
|
||||
struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg;
|
||||
|
||||
int poll_count = 0;
|
||||
struct sdlog_sysvector sysvect;
|
||||
memset(&sysvect, 0, sizeof(sysvect));
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* make sure threads are synchronized */
|
||||
pthread_mutex_lock(&sysvector_mutex);
|
||||
|
||||
/* only wait if no data is available to process */
|
||||
if (sdlog_logbuffer_is_empty(logbuf)) {
|
||||
/* blocking wait for new data at this line */
|
||||
pthread_cond_wait(&sysvector_cond, &sysvector_mutex);
|
||||
}
|
||||
|
||||
/* only quickly load data, do heavy I/O a few lines down */
|
||||
int ret = sdlog_logbuffer_read(logbuf, &sysvect);
|
||||
/* continue */
|
||||
pthread_mutex_unlock(&sysvector_mutex);
|
||||
|
||||
if (ret == OK) {
|
||||
sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect));
|
||||
}
|
||||
|
||||
if (poll_count % 100 == 0) {
|
||||
fsync(sysvector_file);
|
||||
}
|
||||
|
||||
poll_count++;
|
||||
}
|
||||
|
||||
fsync(sysvector_file);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
sysvector_write_start(struct sdlog_logbuffer *logbuf)
|
||||
{
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
struct sched_param param;
|
||||
/* low priority, as this is expensive disk I/O */
|
||||
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf);
|
||||
return thread;
|
||||
|
||||
// XXX we have to destroy the attr at some point
|
||||
}
|
||||
|
||||
|
||||
int sdlog_thread_main(int argc, char *argv[])
|
||||
{
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
if (mavlink_fd < 0) {
|
||||
warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
|
||||
}
|
||||
|
||||
/* log every n'th value (skip three per default) */
|
||||
int skip_value = 3;
|
||||
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
int ch;
|
||||
|
||||
while ((ch = getopt(argc, argv, "s:r")) != EOF) {
|
||||
switch (ch) {
|
||||
case 's':
|
||||
{
|
||||
/* log only every n'th (gyro clocked) value */
|
||||
unsigned s = strtoul(optarg, NULL, 10);
|
||||
|
||||
if (s < 1 || s > 250) {
|
||||
errx(1, "Wrong skip value of %d, out of range (1..250)\n", s);
|
||||
} else {
|
||||
skip_value = s;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case 'r':
|
||||
/* log only on request, disable logging per default */
|
||||
logging_enabled = false;
|
||||
break;
|
||||
|
||||
case '?':
|
||||
if (optopt == 'c') {
|
||||
warnx("Option -%c requires an argument.\n", optopt);
|
||||
} else if (isprint(optopt)) {
|
||||
warnx("Unknown option `-%c'.\n", optopt);
|
||||
} else {
|
||||
warnx("Unknown option character `\\x%x'.\n", optopt);
|
||||
}
|
||||
|
||||
default:
|
||||
usage("unrecognized flag");
|
||||
errx(1, "exiting.");
|
||||
}
|
||||
}
|
||||
|
||||
if (file_exist(mountpoint) != OK) {
|
||||
errx(1, "logging mount point %s not present, exiting.", mountpoint);
|
||||
}
|
||||
|
||||
char folder_path[64];
|
||||
|
||||
if (create_logfolder(folder_path))
|
||||
errx(1, "unable to create logging folder, exiting.");
|
||||
|
||||
FILE *gpsfile;
|
||||
FILE *blackbox_file;
|
||||
|
||||
/* string to hold the path to the sensorfile */
|
||||
char path_buf[64] = "";
|
||||
|
||||
/* only print logging path, important to find log file later */
|
||||
warnx("logging to directory %s\n", folder_path);
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
|
||||
sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
|
||||
|
||||
if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
|
||||
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
|
||||
|
||||
if (NULL == (gpsfile = fopen(path_buf, "w"))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
int gpsfile_no = fileno(gpsfile);
|
||||
|
||||
/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
|
||||
sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
|
||||
|
||||
if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
|
||||
errx(1, "opening %s failed.\n", path_buf);
|
||||
}
|
||||
|
||||
// XXX for fsync() calls
|
||||
int blackbox_file_no = fileno(blackbox_file);
|
||||
|
||||
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
|
||||
/* number of messages */
|
||||
const ssize_t fdsc = 25;
|
||||
/* Sanity check variable and index */
|
||||
ssize_t fdsc_count = 0;
|
||||
/* file descriptors to wait for */
|
||||
struct pollfd fds[fdsc];
|
||||
|
||||
|
||||
struct {
|
||||
struct sensor_combined_s raw;
|
||||
struct vehicle_attitude_s att;
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
struct actuator_outputs_s act_outputs;
|
||||
struct actuator_controls_s act_controls;
|
||||
struct actuator_controls_effective_s act_controls_effective;
|
||||
struct vehicle_command_s cmd;
|
||||
struct vehicle_local_position_s local_pos;
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct vehicle_gps_position_s gps_pos;
|
||||
struct vehicle_vicon_position_s vicon_pos;
|
||||
struct optical_flow_s flow;
|
||||
struct battery_status_s batt;
|
||||
struct differential_pressure_s diff_pres;
|
||||
struct airspeed_s airspeed;
|
||||
} buf;
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
|
||||
struct {
|
||||
int cmd_sub;
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
int spa_sub;
|
||||
int act_0_sub;
|
||||
int controls_0_sub;
|
||||
int controls_effective_0_sub;
|
||||
int local_pos_sub;
|
||||
int global_pos_sub;
|
||||
int gps_pos_sub;
|
||||
int vicon_pos_sub;
|
||||
int flow_sub;
|
||||
int batt_sub;
|
||||
int diff_pres_sub;
|
||||
int airspeed_sub;
|
||||
} subs;
|
||||
|
||||
/* --- MANAGEMENT - LOGGING COMMAND --- */
|
||||
/* subscribe to ORB for vehicle command */
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
fds[fdsc_count].fd = subs.cmd_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- GPS POSITION --- */
|
||||
/* subscribe to ORB for global position */
|
||||
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
fds[fdsc_count].fd = subs.gps_pos_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
/* subscribe to ORB for sensors raw */
|
||||
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
fds[fdsc_count].fd = subs.sensor_sub;
|
||||
/* do not rate limit, instead use skip counter (aliasing on rate limit) */
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- ATTITUDE VALUE --- */
|
||||
/* subscribe to ORB for attitude */
|
||||
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
fds[fdsc_count].fd = subs.att_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- ATTITUDE SETPOINT VALUE --- */
|
||||
/* subscribe to ORB for attitude setpoint */
|
||||
/* struct already allocated */
|
||||
subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
fds[fdsc_count].fd = subs.spa_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/** --- ACTUATOR OUTPUTS --- */
|
||||
subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
|
||||
fds[fdsc_count].fd = subs.act_0_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- ACTUATOR CONTROL VALUE --- */
|
||||
/* subscribe to ORB for actuator control */
|
||||
subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
fds[fdsc_count].fd = subs.controls_0_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
|
||||
/* subscribe to ORB for actuator control */
|
||||
subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
|
||||
fds[fdsc_count].fd = subs.controls_effective_0_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- LOCAL POSITION --- */
|
||||
/* subscribe to ORB for local position */
|
||||
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
fds[fdsc_count].fd = subs.local_pos_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- GLOBAL POSITION --- */
|
||||
/* subscribe to ORB for global position */
|
||||
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
fds[fdsc_count].fd = subs.global_pos_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
/* subscribe to ORB for vicon position */
|
||||
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
fds[fdsc_count].fd = subs.vicon_pos_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- FLOW measurements --- */
|
||||
/* subscribe to ORB for flow measurements */
|
||||
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
|
||||
fds[fdsc_count].fd = subs.flow_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- BATTERY STATUS --- */
|
||||
/* subscribe to ORB for flow measurements */
|
||||
subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
fds[fdsc_count].fd = subs.batt_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- DIFFERENTIAL PRESSURE --- */
|
||||
/* subscribe to ORB for flow measurements */
|
||||
subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
fds[fdsc_count].fd = subs.diff_pres_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- AIRSPEED --- */
|
||||
/* subscribe to ORB for airspeed */
|
||||
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
fds[fdsc_count].fd = subs.airspeed_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* WARNING: If you get the error message below,
|
||||
* then the number of registered messages (fdsc)
|
||||
* differs from the number of messages in the above list.
|
||||
*/
|
||||
if (fdsc_count > fdsc) {
|
||||
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
|
||||
fdsc_count = fdsc;
|
||||
}
|
||||
|
||||
/*
|
||||
* set up poll to block for new data,
|
||||
* wait for a maximum of 1000 ms (1 second)
|
||||
*/
|
||||
// const int timeout = 1000;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* initialize log buffer with a size of 10 */
|
||||
sdlog_logbuffer_init(&lb, 10);
|
||||
|
||||
/* initialize thread synchronization */
|
||||
pthread_mutex_init(&sysvector_mutex, NULL);
|
||||
pthread_cond_init(&sysvector_cond, NULL);
|
||||
|
||||
/* start logbuffer emptying thread */
|
||||
pthread_t sysvector_pthread = sysvector_write_start(&lb);
|
||||
|
||||
starttime = hrt_absolute_time();
|
||||
|
||||
/* track skipping */
|
||||
int skip_count = 0;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* only poll for commands, gps and sensor_combined */
|
||||
int poll_ret = poll(fds, 3, 1000);
|
||||
|
||||
/* handle the poll result */
|
||||
if (poll_ret == 0) {
|
||||
/* XXX this means none of our providers is giving us data - might be an error? */
|
||||
} else if (poll_ret < 0) {
|
||||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else {
|
||||
|
||||
int ifds = 0;
|
||||
|
||||
/* --- VEHICLE COMMAND VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* copy command into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
|
||||
|
||||
/* always log to blackbox, even when logging disabled */
|
||||
blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
|
||||
buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
|
||||
(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
|
||||
|
||||
handle_command(&buf.cmd);
|
||||
}
|
||||
|
||||
/* --- VEHICLE GPS VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
/* copy gps position into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
|
||||
|
||||
/* if logging disabled, continue */
|
||||
if (logging_enabled) {
|
||||
|
||||
/* write KML line */
|
||||
}
|
||||
}
|
||||
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
if (fds[ifds++].revents & POLLIN) {
|
||||
|
||||
// /* copy sensors raw data into local buffer */
|
||||
// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
// /* write out */
|
||||
// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
|
||||
|
||||
/* always copy sensors raw data into local buffer, since poll flags won't clear else */
|
||||
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
|
||||
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
|
||||
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
|
||||
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
|
||||
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
|
||||
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
|
||||
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
|
||||
orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
|
||||
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
|
||||
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
|
||||
|
||||
/* if skipping is on or logging is disabled, ignore */
|
||||
if (skip_count < skip_value || !logging_enabled) {
|
||||
skip_count++;
|
||||
/* do not log data */
|
||||
continue;
|
||||
} else {
|
||||
/* log data, reset */
|
||||
skip_count = 0;
|
||||
}
|
||||
|
||||
struct sdlog_sysvector sysvect = {
|
||||
.timestamp = buf.raw.timestamp,
|
||||
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
|
||||
.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
|
||||
.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
|
||||
.baro = buf.raw.baro_pres_mbar,
|
||||
.baro_alt = buf.raw.baro_alt_meter,
|
||||
.baro_temp = buf.raw.baro_temp_celcius,
|
||||
.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
|
||||
.actuators = {
|
||||
buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
|
||||
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
|
||||
},
|
||||
.vbat = buf.batt.voltage_v,
|
||||
.bat_current = buf.batt.current_a,
|
||||
.bat_discharged = buf.batt.discharged_mah,
|
||||
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
|
||||
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
|
||||
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
|
||||
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
|
||||
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
|
||||
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
|
||||
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
|
||||
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
|
||||
.diff_pressure = buf.diff_pres.differential_pressure_pa,
|
||||
.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
|
||||
.true_airspeed = buf.airspeed.true_airspeed_m_s
|
||||
};
|
||||
|
||||
/* put into buffer for later IO */
|
||||
pthread_mutex_lock(&sysvector_mutex);
|
||||
sdlog_logbuffer_write(&lb, &sysvect);
|
||||
/* signal the other thread new data, but not yet unlock */
|
||||
if ((unsigned)lb.count > (lb.size / 2)) {
|
||||
/* only request write if several packets can be written at once */
|
||||
pthread_cond_signal(&sysvector_cond);
|
||||
}
|
||||
/* unlock, now the writer thread may run */
|
||||
pthread_mutex_unlock(&sysvector_mutex);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
print_sdlog_status();
|
||||
|
||||
/* wake up write thread one last time */
|
||||
pthread_mutex_lock(&sysvector_mutex);
|
||||
pthread_cond_signal(&sysvector_cond);
|
||||
/* unlock, now the writer thread may return */
|
||||
pthread_mutex_unlock(&sysvector_mutex);
|
||||
|
||||
/* wait for write thread to return */
|
||||
(void)pthread_join(sysvector_pthread, NULL);
|
||||
|
||||
pthread_mutex_destroy(&sysvector_mutex);
|
||||
pthread_cond_destroy(&sysvector_cond);
|
||||
|
||||
warnx("exiting.\n\n");
|
||||
|
||||
/* finish KML file */
|
||||
// XXX
|
||||
fclose(gpsfile);
|
||||
fclose(blackbox_file);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void print_sdlog_status()
|
||||
{
|
||||
unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
|
||||
float mebibytes = bytes / 1024.0f / 1024.0f;
|
||||
float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
|
||||
|
||||
warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
|
||||
}
|
||||
|
||||
/**
|
||||
* @return 0 if file exists
|
||||
*/
|
||||
int file_exist(const char *filename)
|
||||
{
|
||||
struct stat buffer;
|
||||
return stat(filename, &buffer);
|
||||
}
|
||||
|
||||
int file_copy(const char *file_old, const char *file_new)
|
||||
{
|
||||
FILE *source, *target;
|
||||
source = fopen(file_old, "r");
|
||||
int ret = 0;
|
||||
|
||||
if (source == NULL) {
|
||||
warnx("failed opening input file to copy");
|
||||
return 1;
|
||||
}
|
||||
|
||||
target = fopen(file_new, "w");
|
||||
|
||||
if (target == NULL) {
|
||||
fclose(source);
|
||||
warnx("failed to open output file to copy");
|
||||
return 1;
|
||||
}
|
||||
|
||||
char buf[128];
|
||||
int nread;
|
||||
|
||||
while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
|
||||
int ret = fwrite(buf, 1, nread, target);
|
||||
|
||||
if (ret <= 0) {
|
||||
warnx("error writing file");
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
fsync(fileno(target));
|
||||
|
||||
fclose(source);
|
||||
fclose(target);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void handle_command(struct vehicle_command_s *cmd)
|
||||
{
|
||||
/* result of the command */
|
||||
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
|
||||
/* request to set different system mode */
|
||||
switch (cmd->command) {
|
||||
|
||||
case VEHICLE_CMD_PREFLIGHT_STORAGE:
|
||||
|
||||
if (((int)(cmd->param3)) == 1) {
|
||||
|
||||
/* enable logging */
|
||||
mavlink_log_info(mavlink_fd, "[log] file:");
|
||||
mavlink_log_info(mavlink_fd, "logdir");
|
||||
logging_enabled = true;
|
||||
}
|
||||
if (((int)(cmd->param3)) == 0) {
|
||||
|
||||
/* disable logging */
|
||||
mavlink_log_info(mavlink_fd, "[log] stopped.");
|
||||
logging_enabled = false;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
/* silently ignore */
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
|
@ -1,91 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: 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 sdlog_log.c
|
||||
* MAVLink text logging.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "sdlog_ringbuffer.h"
|
||||
|
||||
void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size)
|
||||
{
|
||||
lb->size = size;
|
||||
lb->start = 0;
|
||||
lb->count = 0;
|
||||
lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector));
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb)
|
||||
{
|
||||
return lb->count == (int)lb->size;
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb)
|
||||
{
|
||||
return lb->count == 0;
|
||||
}
|
||||
|
||||
|
||||
// XXX make these functions thread-safe
|
||||
void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem)
|
||||
{
|
||||
int end = (lb->start + lb->count) % lb->size;
|
||||
memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector));
|
||||
|
||||
if (sdlog_logbuffer_is_full(lb)) {
|
||||
lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
|
||||
|
||||
} else {
|
||||
++lb->count;
|
||||
}
|
||||
}
|
||||
|
||||
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem)
|
||||
{
|
||||
if (!sdlog_logbuffer_is_empty(lb)) {
|
||||
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector));
|
||||
lb->start = (lb->start + 1) % lb->size;
|
||||
--lb->count;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
|
@ -1,91 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: 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 sdlog_ringbuffer.h
|
||||
* microSD logging
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef SDLOG_RINGBUFFER_H_
|
||||
#define SDLOG_RINGBUFFER_H_
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct sdlog_sysvector {
|
||||
uint64_t timestamp; /**< time [us] */
|
||||
float gyro[3]; /**< [rad/s] */
|
||||
float accel[3]; /**< [m/s^2] */
|
||||
float mag[3]; /**< [gauss] */
|
||||
float baro; /**< pressure [millibar] */
|
||||
float baro_alt; /**< altitude above MSL [meter] */
|
||||
float baro_temp; /**< [degree celcius] */
|
||||
float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
|
||||
float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
|
||||
float vbat; /**< battery voltage in [volt] */
|
||||
float bat_current; /**< battery discharge current */
|
||||
float bat_discharged; /**< discharged energy in mAh */
|
||||
float adc[4]; /**< ADC ports [volt] */
|
||||
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
|
||||
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
|
||||
float attitude[3]; /**< roll, pitch, yaw [rad] */
|
||||
float rotMatrix[9]; /**< unitvectors */
|
||||
float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */
|
||||
float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
|
||||
float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
|
||||
float diff_pressure; /**< differential pressure */
|
||||
float ind_airspeed; /**< indicated airspeed */
|
||||
float true_airspeed; /**< true airspeed */
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
struct sdlog_logbuffer {
|
||||
unsigned int start;
|
||||
// unsigned int end;
|
||||
unsigned int size;
|
||||
int count;
|
||||
struct sdlog_sysvector *elems;
|
||||
};
|
||||
|
||||
void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size);
|
||||
|
||||
int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb);
|
||||
|
||||
int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb);
|
||||
|
||||
void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem);
|
||||
|
||||
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue