mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge branch 'master' of https://github.com/PX4/Firmware
This commit is contained in:
Thomas Gubler
commit
b9510b89bb
6
Makefile
6
Makefile
@ -114,6 +114,10 @@ endif
|
||||
|
||||
upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
|
||||
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
|
||||
|
||||
upload-jtag-px4fmu:
|
||||
@echo Attempting to flash PX4FMU board via JTAG
|
||||
@openocd -f interface/olimex-jtag-tiny.cfg -f Tools/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
|
||||
|
||||
#
|
||||
# Hacks and fixups
|
||||
@ -129,7 +133,7 @@ endif
|
||||
# a complete re-compilation, 'distclean' should remove everything
|
||||
# that's generated leaving only files that are in source control.
|
||||
#
|
||||
.PHONY: clean
|
||||
.PHONY: clean upload-jtag-px4fmu
|
||||
clean:
|
||||
@make -C $(NUTTX_SRC) -r $(MQUIET) distclean
|
||||
@make -C $(ROMFS_SRC) -r $(MQUIET) clean
|
||||
|
||||
@ -2,69 +2,75 @@
|
||||
#
|
||||
# Flight startup script for PX4FMU on PX4IOAR carrier board.
|
||||
#
|
||||
|
||||
|
||||
set USB no
|
||||
|
||||
echo "[init] doing PX4IOAR startup..."
|
||||
|
||||
|
||||
#
|
||||
# Start the ORB
|
||||
#
|
||||
uorb start
|
||||
|
||||
|
||||
#
|
||||
# Init the EEPROM
|
||||
#
|
||||
echo "[init] eeprom"
|
||||
eeprom start
|
||||
if [ -f /eeprom/parameters ]
|
||||
then
|
||||
eeprom load_param /eeprom/parameters
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensors.
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
|
||||
#
|
||||
# Start MAVLink
|
||||
#
|
||||
mavlink start -d /dev/ttyS0 -b 57600
|
||||
usleep 5000
|
||||
|
||||
|
||||
#
|
||||
# Start the commander.
|
||||
#
|
||||
# XXX this should be '<command> start'.
|
||||
#
|
||||
commander &
|
||||
|
||||
commander start
|
||||
|
||||
#
|
||||
# Start the attitude estimator
|
||||
#
|
||||
# XXX this should be '<command> start'.
|
||||
#
|
||||
attitude_estimator_bm &
|
||||
#position_estimator &
|
||||
|
||||
attitude_estimator_ekf start
|
||||
|
||||
#
|
||||
# Configure PX4FMU for operation with PX4IOAR
|
||||
#
|
||||
# XXX arguments?
|
||||
fmu mode_gpio_serial
|
||||
|
||||
#
|
||||
#fmu mode_
|
||||
# Fire up the multi rotor attitude controller
|
||||
#
|
||||
multirotor_att_control start
|
||||
|
||||
#
|
||||
# Fire up the AR.Drone interface.
|
||||
#
|
||||
ardrone_interface start
|
||||
|
||||
#
|
||||
# Fire up the AR.Drone controller.
|
||||
# Start logging
|
||||
#
|
||||
# XXX this should be '<command> start'.
|
||||
#
|
||||
ardrone_control -d /dev/ttyS1 -m attitude &
|
||||
#sdlog start
|
||||
|
||||
#
|
||||
# Start looking for a GPS.
|
||||
# Start GPS capture
|
||||
#
|
||||
# XXX this should not need to be backgrounded
|
||||
#
|
||||
#gps -d /dev/ttyS3 -m all &
|
||||
|
||||
#
|
||||
# Start logging to microSD if we can
|
||||
#
|
||||
#sh /etc/init.d/rc.logging
|
||||
|
||||
gps start
|
||||
|
||||
#
|
||||
# startup is done; we don't want the shell because we
|
||||
# use the same UART for telemetry (dumb).
|
||||
# use the same UART for telemetry
|
||||
#
|
||||
echo "[init] startup done, exiting."
|
||||
exit
|
||||
echo "[init] startup done"
|
||||
exit
|
||||
@ -60,6 +60,7 @@ import json
|
||||
import zlib
|
||||
import base64
|
||||
import time
|
||||
import array
|
||||
|
||||
from sys import platform as _platform
|
||||
|
||||
@ -67,7 +68,41 @@ class firmware(object):
|
||||
'''Loads a firmware file'''
|
||||
|
||||
desc = {}
|
||||
image = bytearray()
|
||||
image = bytes()
|
||||
crctab = array.array('I', [
|
||||
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
|
||||
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
|
||||
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
|
||||
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
|
||||
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
|
||||
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
|
||||
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
|
||||
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
|
||||
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
|
||||
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
|
||||
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
|
||||
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
|
||||
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
|
||||
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
|
||||
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
|
||||
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
|
||||
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
|
||||
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
|
||||
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
|
||||
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
|
||||
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
|
||||
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
|
||||
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
|
||||
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
|
||||
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
|
||||
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
|
||||
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
|
||||
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
|
||||
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
|
||||
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
|
||||
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
|
||||
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ])
|
||||
crcpad = bytearray('\xff\xff\xff\xff')
|
||||
|
||||
def __init__(self, path):
|
||||
|
||||
@ -76,30 +111,48 @@ class firmware(object):
|
||||
self.desc = json.load(f)
|
||||
f.close()
|
||||
|
||||
self.image = zlib.decompress(base64.b64decode(self.desc['image']))
|
||||
self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
|
||||
|
||||
# pad image to 4-byte length
|
||||
while ((len(self.image) % 4) != 0):
|
||||
self.image += b'\x00'
|
||||
self.image.append('\xff')
|
||||
|
||||
def property(self, propname):
|
||||
return self.desc[propname]
|
||||
|
||||
def __crc32(self, bytes, state):
|
||||
for byte in bytes:
|
||||
index = (state ^ byte) & 0xff
|
||||
state = self.crctab[index] ^ (state >> 8)
|
||||
return state
|
||||
|
||||
def crc(self, padlen):
|
||||
state = self.__crc32(self.image, int(0))
|
||||
for i in range(len(self.image), (padlen - 1), 4):
|
||||
state = self.__crc32(self.crcpad, state)
|
||||
return state
|
||||
|
||||
class uploader(object):
|
||||
'''Uploads a firmware file to the PX FMU bootloader'''
|
||||
|
||||
NOP = chr(0x00)
|
||||
OK = chr(0x10)
|
||||
FAILED = chr(0x11)
|
||||
# protocol bytes
|
||||
INSYNC = chr(0x12)
|
||||
EOC = chr(0x20)
|
||||
|
||||
# reply bytes
|
||||
OK = chr(0x10)
|
||||
FAILED = chr(0x11)
|
||||
INVALID = chr(0x13) # rev3+
|
||||
|
||||
# command bytes
|
||||
NOP = chr(0x00) # guaranteed to be discarded by the bootloader
|
||||
GET_SYNC = chr(0x21)
|
||||
GET_DEVICE = chr(0x22)
|
||||
CHIP_ERASE = chr(0x23)
|
||||
CHIP_VERIFY = chr(0x24)
|
||||
CHIP_VERIFY = chr(0x24) # rev2 only
|
||||
PROG_MULTI = chr(0x27)
|
||||
READ_MULTI = chr(0x28)
|
||||
READ_MULTI = chr(0x28) # rev2 only
|
||||
GET_CRC = chr(0x29) # rev3+
|
||||
REBOOT = chr(0x30)
|
||||
|
||||
INFO_BL_REV = chr(1) # bootloader protocol revision
|
||||
@ -126,19 +179,28 @@ class uploader(object):
|
||||
|
||||
def __recv(self, count = 1):
|
||||
c = self.port.read(count)
|
||||
if (len(c) < 1):
|
||||
if len(c) < 1:
|
||||
raise RuntimeError("timeout waiting for data")
|
||||
# print("recv " + binascii.hexlify(c))
|
||||
return c
|
||||
|
||||
def __recv_int(self):
|
||||
raw = self.__recv(4)
|
||||
val = struct.unpack("<I", raw)
|
||||
return val[0]
|
||||
|
||||
def __getSync(self):
|
||||
self.port.flush()
|
||||
c = self.__recv()
|
||||
if (c != self.INSYNC):
|
||||
if c is not self.INSYNC:
|
||||
raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
|
||||
c = self.__recv()
|
||||
if (c != self.OK):
|
||||
raise RuntimeError("unexpected 0x%x instead of OK" % ord(c))
|
||||
if c == self.INVALID:
|
||||
raise RuntimeError("bootloader reports INVALID OPERATION")
|
||||
if c == self.FAILED:
|
||||
raise RuntimeError("bootloader reports OPERATION FAILED")
|
||||
if c != self.OK:
|
||||
raise RuntimeError("unexpected response 0x%x instead of OK" % ord(c))
|
||||
|
||||
# attempt to get back into sync with the bootloader
|
||||
def __sync(self):
|
||||
@ -164,10 +226,9 @@ class uploader(object):
|
||||
# send the GET_DEVICE command and wait for an info parameter
|
||||
def __getInfo(self, param):
|
||||
self.__send(uploader.GET_DEVICE + param + uploader.EOC)
|
||||
raw = self.__recv(4)
|
||||
value = self.__recv_int()
|
||||
self.__getSync()
|
||||
value = struct.unpack_from('<I', raw)
|
||||
return value[0]
|
||||
return value
|
||||
|
||||
# send the CHIP_ERASE command and wait for the bootloader to become ready
|
||||
def __erase(self):
|
||||
@ -200,7 +261,7 @@ class uploader(object):
|
||||
+ uploader.EOC)
|
||||
self.port.flush()
|
||||
programmed = self.__recv(len(data))
|
||||
if (programmed != data):
|
||||
if programmed != data:
|
||||
print("got " + binascii.hexlify(programmed))
|
||||
print("expect " + binascii.hexlify(data))
|
||||
return False
|
||||
@ -209,9 +270,14 @@ class uploader(object):
|
||||
|
||||
# send the reboot command
|
||||
def __reboot(self):
|
||||
self.__send(uploader.REBOOT)
|
||||
self.__send(uploader.REBOOT
|
||||
+ uploader.EOC)
|
||||
self.port.flush()
|
||||
|
||||
# v3+ can report failure if the first word flash fails
|
||||
if self.bl_rev >= 3:
|
||||
self.__getSync()
|
||||
|
||||
# split a sequence into a list of size-constrained pieces
|
||||
def __split_len(self, seq, length):
|
||||
return [seq[i:i+length] for i in range(0, len(seq), length)]
|
||||
@ -224,7 +290,7 @@ class uploader(object):
|
||||
self.__program_multi(bytes)
|
||||
|
||||
# verify code
|
||||
def __verify(self, fw):
|
||||
def __verify_v2(self, fw):
|
||||
self.__send(uploader.CHIP_VERIFY
|
||||
+ uploader.EOC)
|
||||
self.__getSync()
|
||||
@ -234,14 +300,25 @@ class uploader(object):
|
||||
if (not self.__verify_multi(bytes)):
|
||||
raise RuntimeError("Verification failed")
|
||||
|
||||
def __verify_v3(self, fw):
|
||||
expect_crc = fw.crc(self.fw_maxsize)
|
||||
self.__send(uploader.GET_CRC
|
||||
+ uploader.EOC)
|
||||
report_crc = self.__recv_int()
|
||||
self.__getSync()
|
||||
if report_crc != expect_crc:
|
||||
print("Expected 0x%x" % expect_crc)
|
||||
print("Got 0x%x" % report_crc)
|
||||
raise RuntimeError("Program CRC failed")
|
||||
|
||||
# get basic data about the board
|
||||
def identify(self):
|
||||
# make sure we are in sync before starting
|
||||
self.__sync()
|
||||
|
||||
# get the bootloader protocol ID first
|
||||
bl_rev = self.__getInfo(uploader.INFO_BL_REV)
|
||||
if (bl_rev < uploader.BL_REV_MIN) or (bl_rev > uploader.BL_REV_MAX):
|
||||
self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
|
||||
if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
|
||||
print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
|
||||
raise RuntimeError("Bootloader protocol mismatch")
|
||||
|
||||
@ -264,7 +341,10 @@ class uploader(object):
|
||||
self.__program(fw)
|
||||
|
||||
print("verify...")
|
||||
self.__verify(fw)
|
||||
if self.bl_rev == 2:
|
||||
self.__verify_v2(fw)
|
||||
else:
|
||||
self.__verify_v3(fw)
|
||||
|
||||
print("done, rebooting.")
|
||||
self.__reboot()
|
||||
@ -313,7 +393,7 @@ while True:
|
||||
try:
|
||||
# identify the bootloader
|
||||
up.identify()
|
||||
print("Found board %x,%x on %s" % (up.board_type, up.board_rev, port))
|
||||
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
|
||||
|
||||
except:
|
||||
# most probably a timeout talking to the port, no bootloader
|
||||
|
||||
@ -56,6 +56,8 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "ardrone_motor_control.h"
|
||||
|
||||
__EXPORT int ardrone_interface_main(int argc, char *argv[]);
|
||||
@ -112,7 +114,12 @@ int ardrone_interface_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
ardrone_interface_task = task_spawn("ardrone_interface",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
4096,
|
||||
ardrone_interface_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
@ -45,9 +45,9 @@ CSRCS = attitude_estimator_ekf_main.c \
|
||||
codegen/rtGetInf.c \
|
||||
codegen/rtGetNaN.c \
|
||||
codegen/norm.c \
|
||||
codegen/find.c \
|
||||
codegen/diag.c \
|
||||
codegen/cross.c
|
||||
codegen/cross.c \
|
||||
codegen/power.c
|
||||
|
||||
# XXX this is *horribly* broken
|
||||
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
|
||||
|
||||
@ -60,6 +60,8 @@
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <arch/board/up_hrt.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "codegen/attitudeKalmanfilter_initialize.h"
|
||||
#include "codegen/attitudeKalmanfilter.h"
|
||||
|
||||
@ -73,38 +75,45 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
|
||||
|
||||
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
static float dt = 1;
|
||||
static float dt = 1.0f;
|
||||
/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
|
||||
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
|
||||
static float z_k[9] = {0}; /**< Measurement vector */
|
||||
static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */
|
||||
static float x_aposteriori[9] = {0};
|
||||
static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
static float z_k[9]; /**< Measurement vector */
|
||||
static float x_aposteriori_k[12]; /**< */
|
||||
static float x_aposteriori[12];
|
||||
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||
};
|
||||
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||
}; /**< init: diagonal matrix with big values */
|
||||
static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
static float Rot_matrix[9] = {1.f, 0, 0,
|
||||
0, 1.f, 0,
|
||||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
|
||||
@ -150,7 +159,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
20000,
|
||||
attitude_estimator_ekf_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -222,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
||||
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
|
||||
|
||||
|
||||
/* process noise covariance */
|
||||
float q[12];
|
||||
/* measurement noise covariance */
|
||||
float r[9];
|
||||
/* output euler angles */
|
||||
float euler[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* Main loop*/
|
||||
while (!thread_should_exit) {
|
||||
|
||||
@ -271,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
overloadcounter++;
|
||||
}
|
||||
|
||||
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
|
||||
float euler[3];
|
||||
uint8_t update_vect[3] = {1, 1, 1};
|
||||
int32_t z_k_sizes = 9;
|
||||
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
static bool const_initialized = false;
|
||||
|
||||
@ -282,21 +303,42 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
|
||||
{
|
||||
dt = 0.005f;
|
||||
knownConst[0] = 0.6f*0.6f*dt;
|
||||
knownConst[1] = 0.6f*0.6f*dt;
|
||||
knownConst[2] = 0.2f*0.2f*dt;
|
||||
knownConst[3] = 0.2f*0.2f*dt;
|
||||
knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1};
|
||||
q[0] = 1e1f;
|
||||
q[1] = 1e1f;
|
||||
q[2] = 1e1f;
|
||||
/* process noise gyro offset covariance */
|
||||
q[3] = 1e-4f;
|
||||
q[4] = 1e-4f;
|
||||
q[5] = 1e-4f;
|
||||
q[6] = 1e-1f;
|
||||
q[7] = 1e-1f;
|
||||
q[8] = 1e-1f;
|
||||
q[9] = 1e-1f;
|
||||
q[10] = 1e-1f;
|
||||
q[11] = 1e-1f;
|
||||
|
||||
r[0]= 1e-2f;
|
||||
r[1]= 1e-2f;
|
||||
r[2]= 1e-2f;
|
||||
r[3]= 1e-1f;
|
||||
r[4]= 1e-1f;
|
||||
r[5]= 1e-1f;
|
||||
r[6]= 1e-1f;
|
||||
r[7]= 1e-1f;
|
||||
r[8]= 1e-1f;
|
||||
|
||||
x_aposteriori_k[0] = z_k[0];
|
||||
x_aposteriori_k[1] = z_k[1];
|
||||
x_aposteriori_k[2] = z_k[2];
|
||||
x_aposteriori_k[3] = z_k[3];
|
||||
x_aposteriori_k[4] = z_k[4];
|
||||
x_aposteriori_k[5] = z_k[5];
|
||||
x_aposteriori_k[6] = z_k[6];
|
||||
x_aposteriori_k[7] = z_k[7];
|
||||
x_aposteriori_k[8] = z_k[8];
|
||||
x_aposteriori_k[3] = 0.0f;
|
||||
x_aposteriori_k[4] = 0.0f;
|
||||
x_aposteriori_k[5] = 0.0f;
|
||||
x_aposteriori_k[6] = z_k[3];
|
||||
x_aposteriori_k[7] = z_k[4];
|
||||
x_aposteriori_k[8] = z_k[5];
|
||||
x_aposteriori_k[9] = z_k[6];
|
||||
x_aposteriori_k[10] = z_k[7];
|
||||
x_aposteriori_k[11] = z_k[8];
|
||||
|
||||
const_initialized = true;
|
||||
}
|
||||
@ -306,39 +348,43 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
continue;
|
||||
}
|
||||
|
||||
dt = 0.004f;
|
||||
|
||||
uint64_t timing_start = hrt_absolute_time();
|
||||
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
|
||||
Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
|
||||
// Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
|
||||
euler, Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
/* swap values for next iteration */
|
||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
||||
uint64_t timing_diff = hrt_absolute_time() - timing_start;
|
||||
|
||||
/* print rotation matrix every 200th time */
|
||||
if (printcounter % 2 == 0) {
|
||||
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
|
||||
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
|
||||
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
|
||||
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
|
||||
// /* print rotation matrix every 200th time */
|
||||
// if (printcounter % 200 == 0) {
|
||||
// // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
|
||||
// // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
|
||||
// // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
|
||||
// // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
|
||||
|
||||
|
||||
// }
|
||||
// // }
|
||||
|
||||
// printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
|
||||
// printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]);
|
||||
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
|
||||
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
|
||||
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
|
||||
}
|
||||
// printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
|
||||
// // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
|
||||
// // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
|
||||
// // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
|
||||
// }
|
||||
|
||||
int i = printcounter % 9;
|
||||
// int i = printcounter % 9;
|
||||
|
||||
// for (int i = 0; i < 9; i++) {
|
||||
char name[10];
|
||||
sprintf(name, "xapo #%d", i);
|
||||
memcpy(dbg.key, name, sizeof(dbg.key));
|
||||
dbg.value = x_aposteriori[i];
|
||||
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
|
||||
// // for (int i = 0; i < 9; i++) {
|
||||
// char name[10];
|
||||
// sprintf(name, "xapo #%d", i);
|
||||
// memcpy(dbg.key, name, sizeof(dbg.key));
|
||||
// dbg.value = x_aposteriori[i];
|
||||
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
|
||||
|
||||
printcounter++;
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -29,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
|
||||
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter.h) */
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:42 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -27,7 +27,19 @@
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void diag(const real32_T v[3], real32_T d[9])
|
||||
void b_diag(const real32_T v[9], real32_T d[81])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 81U * sizeof(real32_T));
|
||||
for (j = 0; j < 9; j++) {
|
||||
d[j + 9 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void c_diag(const real32_T v[3], real32_T d[9])
|
||||
{
|
||||
int32_T j;
|
||||
for (j = 0; j < 9; j++) {
|
||||
@ -39,4 +51,28 @@ void diag(const real32_T v[3], real32_T d[9])
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void d_diag(const real32_T v[6], real32_T d[36])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 36U * sizeof(real32_T));
|
||||
for (j = 0; j < 6; j++) {
|
||||
d[j + 6 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void diag(const real32_T v[12], real32_T d[144])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 144U * sizeof(real32_T));
|
||||
for (j = 0; j < 12; j++) {
|
||||
d[j + 12 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (diag.c) */
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -29,6 +29,9 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void diag(const real32_T v[3], real32_T d[9]);
|
||||
extern void b_diag(const real32_T v[9], real32_T d[81]);
|
||||
extern void c_diag(const real32_T v[3], real32_T d[9]);
|
||||
extern void d_diag(const real32_T v[6], real32_T d[36]);
|
||||
extern void diag(const real32_T v[12], real32_T d[144]);
|
||||
#endif
|
||||
/* End of code generation (diag.h) */
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -27,12 +27,12 @@
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void b_eye(real_T I[81])
|
||||
void b_eye(real_T I[144])
|
||||
{
|
||||
int32_T i;
|
||||
memset((void *)&I[0], 0, 81U * sizeof(real_T));
|
||||
for (i = 0; i < 9; i++) {
|
||||
I[i + 9 * i] = 1.0;
|
||||
memset((void *)&I[0], 0, 144U * sizeof(real_T));
|
||||
for (i = 0; i < 12; i++) {
|
||||
I[i + 12 * i] = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -29,7 +29,7 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void b_eye(real_T I[81]);
|
||||
extern void b_eye(real_T I[144]);
|
||||
extern void eye(real_T I[9]);
|
||||
#endif
|
||||
/* End of code generation (eye.h) */
|
||||
|
||||
@ -1,77 +0,0 @@
|
||||
/*
|
||||
* find.c
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "find.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
|
||||
{
|
||||
int32_T idx;
|
||||
int32_T ii;
|
||||
boolean_T exitg1;
|
||||
boolean_T guard1 = FALSE;
|
||||
int32_T i2;
|
||||
int8_T b_i_data[9];
|
||||
idx = 0;
|
||||
i_sizes[0] = 9;
|
||||
ii = 1;
|
||||
exitg1 = 0U;
|
||||
while ((exitg1 == 0U) && (ii <= 9)) {
|
||||
guard1 = FALSE;
|
||||
if (x[ii - 1] != 0) {
|
||||
idx++;
|
||||
i_data[idx - 1] = (real_T)ii;
|
||||
if (idx >= 9) {
|
||||
exitg1 = 1U;
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
|
||||
if (guard1 == TRUE) {
|
||||
ii++;
|
||||
}
|
||||
}
|
||||
|
||||
if (1 > idx) {
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
b_i_data[i2] = (int8_T)i_data[i2];
|
||||
}
|
||||
|
||||
i_sizes[0] = idx;
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
i_data[i2] = (real_T)b_i_data[i2];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (find.c) */
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -21,719 +21,345 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x);
|
||||
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2]);
|
||||
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
|
||||
[81], int32_T x_sizes[2], int32_T ix0);
|
||||
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
|
||||
[2]);
|
||||
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
|
||||
x_sizes[2], int32_T ix0);
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)
|
||||
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
|
||||
{
|
||||
return 0.0F;
|
||||
int32_T rtemp;
|
||||
int32_T k;
|
||||
real32_T b_A[9];
|
||||
real32_T b_B[36];
|
||||
int32_T r1;
|
||||
int32_T r2;
|
||||
int32_T r3;
|
||||
real32_T maxval;
|
||||
real32_T a21;
|
||||
real32_T Y[36];
|
||||
for (rtemp = 0; rtemp < 3; rtemp++) {
|
||||
for (k = 0; k < 3; k++) {
|
||||
b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
|
||||
}
|
||||
}
|
||||
|
||||
for (rtemp = 0; rtemp < 12; rtemp++) {
|
||||
for (k = 0; k < 3; k++) {
|
||||
b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
|
||||
}
|
||||
}
|
||||
|
||||
r1 = 0;
|
||||
r2 = 1;
|
||||
r3 = 2;
|
||||
maxval = (real32_T)fabsf(b_A[0]);
|
||||
a21 = (real32_T)fabsf(b_A[1]);
|
||||
if (a21 > maxval) {
|
||||
maxval = a21;
|
||||
r1 = 1;
|
||||
r2 = 0;
|
||||
}
|
||||
|
||||
if ((real32_T)fabsf(b_A[2]) > maxval) {
|
||||
r1 = 2;
|
||||
r2 = 1;
|
||||
r3 = 0;
|
||||
}
|
||||
|
||||
b_A[r2] /= b_A[r1];
|
||||
b_A[r3] /= b_A[r1];
|
||||
b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
|
||||
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
|
||||
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
|
||||
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
|
||||
if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {
|
||||
rtemp = r2;
|
||||
r2 = r3;
|
||||
r3 = rtemp;
|
||||
}
|
||||
|
||||
b_A[3 + r3] /= b_A[3 + r2];
|
||||
b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
|
||||
for (k = 0; k < 12; k++) {
|
||||
Y[3 * k] = b_B[r1 + 3 * k];
|
||||
Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
|
||||
Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
|
||||
+ r3];
|
||||
Y[2 + 3 * k] /= b_A[6 + r3];
|
||||
Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
|
||||
Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
|
||||
Y[1 + 3 * k] /= b_A[3 + r2];
|
||||
Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
|
||||
Y[3 * k] /= b_A[r1];
|
||||
}
|
||||
|
||||
for (rtemp = 0; rtemp < 3; rtemp++) {
|
||||
for (k = 0; k < 12; k++) {
|
||||
y[k + 12 * rtemp] = Y[rtemp + 3 * k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2])
|
||||
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
|
||||
{
|
||||
int32_T loop_ub;
|
||||
int32_T jy;
|
||||
int32_T iy;
|
||||
real32_T b_A_data[81];
|
||||
int32_T jA;
|
||||
int32_T tmp_data[9];
|
||||
int32_T ipiv_data[9];
|
||||
int32_T ldap1;
|
||||
real32_T b_A[36];
|
||||
int8_T ipiv[6];
|
||||
int32_T j;
|
||||
int32_T mmj;
|
||||
int32_T jj;
|
||||
int32_T jp1j;
|
||||
int32_T c;
|
||||
int32_T ix;
|
||||
real32_T temp;
|
||||
int32_T k;
|
||||
real32_T s;
|
||||
int32_T jrow;
|
||||
int32_T b_loop_ub;
|
||||
int32_T c;
|
||||
loop_ub = A_sizes[0] * A_sizes[1] - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
b_A_data[iy] = A_data[iy];
|
||||
}
|
||||
|
||||
iy = A_sizes[1];
|
||||
jA = A_sizes[1];
|
||||
jA = iy <= jA ? iy : jA;
|
||||
if (jA < 1) {
|
||||
} else {
|
||||
loop_ub = jA - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
tmp_data[iy] = 1 + iy;
|
||||
int32_T loop_ub;
|
||||
real32_T Y[72];
|
||||
for (jy = 0; jy < 6; jy++) {
|
||||
for (iy = 0; iy < 6; iy++) {
|
||||
b_A[iy + 6 * jy] = B[jy + 6 * iy];
|
||||
}
|
||||
|
||||
loop_ub = jA - 1;
|
||||
for (iy = 0; iy <= loop_ub; iy++) {
|
||||
ipiv_data[iy] = tmp_data[iy];
|
||||
}
|
||||
ipiv[jy] = (int8_T)(1 + jy);
|
||||
}
|
||||
|
||||
ldap1 = A_sizes[1] + 1;
|
||||
iy = A_sizes[1] - 1;
|
||||
jA = A_sizes[1];
|
||||
loop_ub = iy <= jA ? iy : jA;
|
||||
for (j = 1; j <= loop_ub; j++) {
|
||||
mmj = (A_sizes[1] - j) + 1;
|
||||
jj = (j - 1) * ldap1;
|
||||
if (mmj < 1) {
|
||||
jA = -1;
|
||||
} else {
|
||||
jA = 0;
|
||||
if (mmj > 1) {
|
||||
ix = jj;
|
||||
temp = (real32_T)fabs(b_A_data[jj]);
|
||||
for (k = 1; k + 1 <= mmj; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(b_A_data[ix]);
|
||||
if (s > temp) {
|
||||
jA = k;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
for (j = 0; j < 5; j++) {
|
||||
mmj = -j;
|
||||
jj = j * 7;
|
||||
jp1j = jj + 1;
|
||||
c = mmj + 6;
|
||||
jy = 0;
|
||||
ix = jj;
|
||||
temp = (real32_T)fabsf(b_A[jj]);
|
||||
for (k = 2; k <= c; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabsf(b_A[ix]);
|
||||
if (s > temp) {
|
||||
jy = k - 1;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
|
||||
if ((real_T)b_A_data[jj + jA] != 0.0) {
|
||||
if (jA != 0) {
|
||||
ipiv_data[j - 1] = j + jA;
|
||||
jrow = j - 1;
|
||||
iy = jrow + jA;
|
||||
for (k = 1; k <= A_sizes[1]; k++) {
|
||||
temp = b_A_data[jrow];
|
||||
b_A_data[jrow] = b_A_data[iy];
|
||||
b_A_data[iy] = temp;
|
||||
jrow += A_sizes[1];
|
||||
iy += A_sizes[1];
|
||||
if ((real_T)b_A[jj + jy] != 0.0) {
|
||||
if (jy != 0) {
|
||||
ipiv[j] = (int8_T)((j + jy) + 1);
|
||||
ix = j;
|
||||
iy = j + jy;
|
||||
for (k = 0; k < 6; k++) {
|
||||
temp = b_A[ix];
|
||||
b_A[ix] = b_A[iy];
|
||||
b_A[iy] = temp;
|
||||
ix += 6;
|
||||
iy += 6;
|
||||
}
|
||||
}
|
||||
|
||||
b_loop_ub = jj + mmj;
|
||||
for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) {
|
||||
b_A_data[jA] /= b_A_data[jj];
|
||||
loop_ub = (jp1j + mmj) + 5;
|
||||
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
|
||||
b_A[iy] /= b_A[jj];
|
||||
}
|
||||
}
|
||||
|
||||
c = A_sizes[1] - j;
|
||||
jA = jj + ldap1;
|
||||
iy = jj + A_sizes[1];
|
||||
for (jrow = 1; jrow <= c; jrow++) {
|
||||
if ((real_T)b_A_data[iy] != 0.0) {
|
||||
temp = b_A_data[iy] * -1.0F;
|
||||
ix = jj;
|
||||
b_loop_ub = (mmj + jA) - 1;
|
||||
for (k = jA; k + 1 <= b_loop_ub; k++) {
|
||||
b_A_data[k] += b_A_data[ix + 1] * temp;
|
||||
c = 5 - j;
|
||||
jy = jj + 6;
|
||||
for (iy = 1; iy <= c; iy++) {
|
||||
if ((real_T)b_A[jy] != 0.0) {
|
||||
temp = b_A[jy] * -1.0F;
|
||||
ix = jp1j;
|
||||
loop_ub = (mmj + jj) + 12;
|
||||
for (k = 7 + jj; k + 1 <= loop_ub; k++) {
|
||||
b_A[k] += b_A[ix] * temp;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
iy += A_sizes[1];
|
||||
jA += A_sizes[1];
|
||||
jy += 6;
|
||||
jj += 6;
|
||||
}
|
||||
}
|
||||
|
||||
for (jA = 0; jA + 1 <= A_sizes[1]; jA++) {
|
||||
if (ipiv_data[jA] != jA + 1) {
|
||||
for (j = 0; j < 9; j++) {
|
||||
temp = B_data[jA + B_sizes[0] * j];
|
||||
B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) -
|
||||
1];
|
||||
B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp;
|
||||
for (jy = 0; jy < 12; jy++) {
|
||||
for (iy = 0; iy < 6; iy++) {
|
||||
Y[iy + 6 * jy] = A[jy + 12 * iy];
|
||||
}
|
||||
}
|
||||
|
||||
for (iy = 0; iy < 6; iy++) {
|
||||
if (ipiv[iy] != iy + 1) {
|
||||
for (j = 0; j < 12; j++) {
|
||||
temp = Y[iy + 6 * j];
|
||||
Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];
|
||||
Y[(ipiv[iy] + 6 * j) - 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (B_sizes[0] == 0) {
|
||||
} else {
|
||||
for (j = 0; j < 9; j++) {
|
||||
c = A_sizes[1] * j;
|
||||
for (k = 0; k + 1 <= A_sizes[1]; k++) {
|
||||
iy = A_sizes[1] * k;
|
||||
if ((real_T)B_data[k + c] != 0.0) {
|
||||
for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) {
|
||||
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
|
||||
}
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 6 * j;
|
||||
for (k = 0; k < 6; k++) {
|
||||
jy = 6 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
for (iy = k + 2; iy < 7; iy++) {
|
||||
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (B_sizes[0] == 0) {
|
||||
} else {
|
||||
for (j = 0; j < 9; j++) {
|
||||
c = A_sizes[1] * j;
|
||||
for (k = A_sizes[1] - 1; k + 1 > 0; k--) {
|
||||
iy = A_sizes[1] * k;
|
||||
if ((real_T)B_data[k + c] != 0.0) {
|
||||
B_data[k + c] /= b_A_data[k + iy];
|
||||
for (jA = 0; jA + 1 <= k; jA++) {
|
||||
B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
|
||||
}
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 6 * j;
|
||||
for (k = 5; k > -1; k += -1) {
|
||||
jy = 6 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
Y[k + c] /= b_A[k + jy];
|
||||
for (iy = 0; iy + 1 <= k; iy++) {
|
||||
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 6; jy++) {
|
||||
for (iy = 0; iy < 12; iy++) {
|
||||
y[iy + 12 * jy] = Y[jy + 6 * iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
|
||||
[81], int32_T x_sizes[2], int32_T ix0)
|
||||
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
|
||||
{
|
||||
real32_T tau;
|
||||
int32_T nm1;
|
||||
real32_T xnorm;
|
||||
real32_T a;
|
||||
int32_T knt;
|
||||
int32_T loop_ub;
|
||||
int32_T k;
|
||||
tau = 0.0F;
|
||||
if (n <= 0) {
|
||||
} else {
|
||||
nm1 = n - 2;
|
||||
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
|
||||
if ((real_T)xnorm != 0.0) {
|
||||
a = (real32_T)fabs(*alpha1);
|
||||
xnorm = (real32_T)fabs(xnorm);
|
||||
if (a < xnorm) {
|
||||
a /= xnorm;
|
||||
xnorm *= (real32_T)sqrt(a * a + 1.0F);
|
||||
} else if (a > xnorm) {
|
||||
xnorm /= a;
|
||||
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
|
||||
} else if (rtIsNaNF(xnorm)) {
|
||||
} else {
|
||||
xnorm = a * 1.41421354F;
|
||||
}
|
||||
|
||||
if ((real_T)*alpha1 >= 0.0) {
|
||||
xnorm = -xnorm;
|
||||
}
|
||||
|
||||
if ((real32_T)fabs(xnorm) < 9.86076132E-32F) {
|
||||
knt = 0;
|
||||
do {
|
||||
knt++;
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= 1.01412048E+31F;
|
||||
}
|
||||
|
||||
xnorm *= 1.01412048E+31F;
|
||||
*alpha1 *= 1.01412048E+31F;
|
||||
} while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F));
|
||||
|
||||
xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
|
||||
a = (real32_T)fabs(*alpha1);
|
||||
xnorm = (real32_T)fabs(xnorm);
|
||||
if (a < xnorm) {
|
||||
a /= xnorm;
|
||||
xnorm *= (real32_T)sqrt(a * a + 1.0F);
|
||||
} else if (a > xnorm) {
|
||||
xnorm /= a;
|
||||
xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
|
||||
} else if (rtIsNaNF(xnorm)) {
|
||||
} else {
|
||||
xnorm = a * 1.41421354F;
|
||||
}
|
||||
|
||||
if ((real_T)*alpha1 >= 0.0) {
|
||||
xnorm = -xnorm;
|
||||
}
|
||||
|
||||
tau = (xnorm - *alpha1) / xnorm;
|
||||
*alpha1 = 1.0F / (*alpha1 - xnorm);
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= *alpha1;
|
||||
}
|
||||
|
||||
for (k = 1; k <= knt; k++) {
|
||||
xnorm *= 9.86076132E-32F;
|
||||
}
|
||||
|
||||
*alpha1 = xnorm;
|
||||
} else {
|
||||
tau = (xnorm - *alpha1) / xnorm;
|
||||
*alpha1 = 1.0F / (*alpha1 - xnorm);
|
||||
loop_ub = ix0 + nm1;
|
||||
for (k = ix0; k <= loop_ub; k++) {
|
||||
x_data[k - 1] *= *alpha1;
|
||||
}
|
||||
|
||||
*alpha1 = xnorm;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return tau;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
|
||||
real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
|
||||
[2])
|
||||
{
|
||||
real_T rankR;
|
||||
real_T u1;
|
||||
int32_T mn;
|
||||
int32_T b_A_sizes[2];
|
||||
int32_T loop_ub;
|
||||
int32_T k;
|
||||
real32_T b_A_data[81];
|
||||
int32_T pvt;
|
||||
int32_T b_mn;
|
||||
int32_T tmp_data[9];
|
||||
int32_T jpvt_data[9];
|
||||
int8_T unnamed_idx_0;
|
||||
real32_T work_data[9];
|
||||
int32_T nmi;
|
||||
real32_T vn1_data[9];
|
||||
real32_T vn2_data[9];
|
||||
int32_T i;
|
||||
int32_T i_i;
|
||||
int32_T mmi;
|
||||
int32_T ix;
|
||||
real32_T smax;
|
||||
real32_T s;
|
||||
int32_T jy;
|
||||
int32_T iy;
|
||||
real32_T atmp;
|
||||
real32_T tau_data[9];
|
||||
int32_T i_ip1;
|
||||
int32_T lastv;
|
||||
int32_T lastc;
|
||||
boolean_T exitg2;
|
||||
int32_T exitg1;
|
||||
boolean_T firstNonZero;
|
||||
real32_T t;
|
||||
rankR = (real_T)A_sizes[0];
|
||||
u1 = (real_T)A_sizes[1];
|
||||
rankR = rankR <= u1 ? rankR : u1;
|
||||
mn = (int32_T)rankR;
|
||||
b_A_sizes[0] = A_sizes[0];
|
||||
b_A_sizes[1] = A_sizes[1];
|
||||
loop_ub = A_sizes[0] * A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
b_A_data[k] = A_data[k];
|
||||
}
|
||||
|
||||
k = A_sizes[0];
|
||||
pvt = A_sizes[1];
|
||||
b_mn = k <= pvt ? k : pvt;
|
||||
if (A_sizes[1] < 1) {
|
||||
} else {
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
tmp_data[k] = 1 + k;
|
||||
}
|
||||
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
jpvt_data[k] = tmp_data[k];
|
||||
}
|
||||
}
|
||||
|
||||
if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) {
|
||||
} else {
|
||||
unnamed_idx_0 = (int8_T)A_sizes[1];
|
||||
loop_ub = unnamed_idx_0 - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
work_data[k] = 0.0F;
|
||||
}
|
||||
|
||||
k = 1;
|
||||
for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) {
|
||||
vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k);
|
||||
vn2_data[nmi] = vn1_data[nmi];
|
||||
k += A_sizes[0];
|
||||
}
|
||||
|
||||
for (i = 0; i + 1 <= b_mn; i++) {
|
||||
i_i = i + i * A_sizes[0];
|
||||
nmi = A_sizes[1] - i;
|
||||
mmi = (A_sizes[0] - i) - 1;
|
||||
if (nmi < 1) {
|
||||
pvt = -1;
|
||||
} else {
|
||||
pvt = 0;
|
||||
if (nmi > 1) {
|
||||
ix = i;
|
||||
smax = (real32_T)fabs(vn1_data[i]);
|
||||
for (k = 1; k + 1 <= nmi; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabs(vn1_data[ix]);
|
||||
if (s > smax) {
|
||||
pvt = k;
|
||||
smax = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pvt += i;
|
||||
if (pvt + 1 != i + 1) {
|
||||
ix = A_sizes[0] * pvt;
|
||||
iy = A_sizes[0] * i;
|
||||
for (k = 1; k <= A_sizes[0]; k++) {
|
||||
s = b_A_data[ix];
|
||||
b_A_data[ix] = b_A_data[iy];
|
||||
b_A_data[iy] = s;
|
||||
ix++;
|
||||
iy++;
|
||||
}
|
||||
|
||||
k = jpvt_data[pvt];
|
||||
jpvt_data[pvt] = jpvt_data[i];
|
||||
jpvt_data[i] = k;
|
||||
vn1_data[pvt] = vn1_data[i];
|
||||
vn2_data[pvt] = vn2_data[i];
|
||||
}
|
||||
|
||||
if (i + 1 < A_sizes[0]) {
|
||||
atmp = b_A_data[i_i];
|
||||
smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2);
|
||||
tau_data[i] = smax;
|
||||
} else {
|
||||
atmp = b_A_data[i_i];
|
||||
smax = b_A_data[i_i];
|
||||
s = b_eml_matlab_zlarfg(&atmp, &smax);
|
||||
b_A_data[i_i] = smax;
|
||||
tau_data[i] = s;
|
||||
}
|
||||
|
||||
b_A_data[i_i] = atmp;
|
||||
if (i + 1 < A_sizes[1]) {
|
||||
atmp = b_A_data[i_i];
|
||||
b_A_data[i_i] = 1.0F;
|
||||
i_ip1 = (i + (i + 1) * A_sizes[0]) + 1;
|
||||
if ((real_T)tau_data[i] != 0.0) {
|
||||
lastv = mmi;
|
||||
pvt = i_i + mmi;
|
||||
while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) {
|
||||
lastv--;
|
||||
pvt--;
|
||||
}
|
||||
|
||||
lastc = nmi - 1;
|
||||
exitg2 = 0U;
|
||||
while ((exitg2 == 0U) && (lastc > 0)) {
|
||||
k = i_ip1 + (lastc - 1) * A_sizes[0];
|
||||
pvt = k + lastv;
|
||||
do {
|
||||
exitg1 = 0U;
|
||||
if (k <= pvt) {
|
||||
if ((real_T)b_A_data[k - 1] != 0.0) {
|
||||
exitg1 = 1U;
|
||||
} else {
|
||||
k++;
|
||||
}
|
||||
} else {
|
||||
lastc--;
|
||||
exitg1 = 2U;
|
||||
}
|
||||
} while (exitg1 == 0U);
|
||||
|
||||
if (exitg1 == 1U) {
|
||||
exitg2 = 1U;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
lastv = -1;
|
||||
lastc = 0;
|
||||
}
|
||||
|
||||
if (lastv + 1 > 0) {
|
||||
if (lastc == 0) {
|
||||
} else {
|
||||
for (iy = 1; iy <= lastc; iy++) {
|
||||
work_data[iy - 1] = 0.0F;
|
||||
}
|
||||
|
||||
iy = 0;
|
||||
loop_ub = i_ip1 + A_sizes[0] * (lastc - 1);
|
||||
pvt = i_ip1;
|
||||
while ((A_sizes[0] > 0) && (pvt <= loop_ub)) {
|
||||
ix = i_i;
|
||||
smax = 0.0F;
|
||||
k = pvt + lastv;
|
||||
for (nmi = pvt; nmi <= k; nmi++) {
|
||||
smax += b_A_data[nmi - 1] * b_A_data[ix];
|
||||
ix++;
|
||||
}
|
||||
|
||||
work_data[iy] += smax;
|
||||
iy++;
|
||||
pvt += A_sizes[0];
|
||||
}
|
||||
}
|
||||
|
||||
smax = -tau_data[i];
|
||||
if ((real_T)smax == 0.0) {
|
||||
} else {
|
||||
pvt = 0;
|
||||
for (nmi = 1; nmi <= lastc; nmi++) {
|
||||
if ((real_T)work_data[pvt] != 0.0) {
|
||||
s = work_data[pvt] * smax;
|
||||
ix = i_i;
|
||||
loop_ub = lastv + i_ip1;
|
||||
for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) {
|
||||
b_A_data[k] += b_A_data[ix] * s;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
pvt++;
|
||||
i_ip1 += A_sizes[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b_A_data[i_i] = atmp;
|
||||
}
|
||||
|
||||
for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) {
|
||||
if ((real_T)vn1_data[nmi] != 0.0) {
|
||||
smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi];
|
||||
smax = 1.0F - smax * smax;
|
||||
if ((real_T)smax < 0.0) {
|
||||
smax = 0.0F;
|
||||
}
|
||||
|
||||
s = vn1_data[nmi] / vn2_data[nmi];
|
||||
if (smax * (s * s) <= 0.000345266977F) {
|
||||
if (i + 1 < A_sizes[0]) {
|
||||
k = (i + A_sizes[0] * nmi) + 1;
|
||||
smax = 0.0F;
|
||||
if (mmi < 1) {
|
||||
} else if (mmi == 1) {
|
||||
smax = (real32_T)fabs(b_A_data[k]);
|
||||
} else {
|
||||
s = 0.0F;
|
||||
firstNonZero = TRUE;
|
||||
pvt = k + mmi;
|
||||
while (k + 1 <= pvt) {
|
||||
if (b_A_data[k] != 0.0F) {
|
||||
atmp = (real32_T)fabs(b_A_data[k]);
|
||||
if (firstNonZero) {
|
||||
s = atmp;
|
||||
smax = 1.0F;
|
||||
firstNonZero = FALSE;
|
||||
} else if (s < atmp) {
|
||||
t = s / atmp;
|
||||
smax = 1.0F + smax * t * t;
|
||||
s = atmp;
|
||||
} else {
|
||||
t = atmp / s;
|
||||
smax += t * t;
|
||||
}
|
||||
}
|
||||
|
||||
k++;
|
||||
}
|
||||
|
||||
smax = s * (real32_T)sqrt(smax);
|
||||
}
|
||||
|
||||
vn1_data[nmi] = smax;
|
||||
vn2_data[nmi] = vn1_data[nmi];
|
||||
} else {
|
||||
vn1_data[nmi] = 0.0F;
|
||||
vn2_data[nmi] = 0.0F;
|
||||
}
|
||||
} else {
|
||||
vn1_data[nmi] *= (real32_T)sqrt(smax);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rankR = (real_T)A_sizes[0];
|
||||
u1 = (real_T)A_sizes[1];
|
||||
rankR = rankR >= u1 ? rankR : u1;
|
||||
smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F;
|
||||
rankR = 0.0;
|
||||
k = 0;
|
||||
while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <=
|
||||
smax))) {
|
||||
rankR++;
|
||||
k++;
|
||||
}
|
||||
|
||||
unnamed_idx_0 = (int8_T)A_sizes[1];
|
||||
Y_sizes[0] = (int32_T)unnamed_idx_0;
|
||||
Y_sizes[1] = 9;
|
||||
loop_ub = unnamed_idx_0 * 9 - 1;
|
||||
for (k = 0; k <= loop_ub; k++) {
|
||||
Y_data[k] = 0.0F;
|
||||
}
|
||||
|
||||
for (nmi = 0; nmi + 1 <= mn; nmi++) {
|
||||
if ((real_T)tau_data[nmi] != 0.0) {
|
||||
for (k = 0; k < 9; k++) {
|
||||
smax = B_data[nmi + B_sizes[0] * k];
|
||||
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
|
||||
smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k];
|
||||
}
|
||||
|
||||
smax *= tau_data[nmi];
|
||||
if ((real_T)smax != 0.0) {
|
||||
B_data[nmi + B_sizes[0] * k] -= smax;
|
||||
for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
|
||||
B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] *
|
||||
smax;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (k = 0; k < 9; k++) {
|
||||
for (i = 0; (real_T)(i + 1) <= rankR; i++) {
|
||||
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k];
|
||||
}
|
||||
|
||||
for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) {
|
||||
Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes
|
||||
[0] * nmi];
|
||||
for (i = 0; i + 1 <= nmi; i++) {
|
||||
Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] +
|
||||
Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
|
||||
x_sizes[2], int32_T ix0)
|
||||
{
|
||||
real32_T y;
|
||||
real32_T scale;
|
||||
boolean_T firstNonZero;
|
||||
int32_T kend;
|
||||
real32_T b_A[81];
|
||||
int8_T ipiv[9];
|
||||
int32_T j;
|
||||
int32_T mmj;
|
||||
int32_T jj;
|
||||
int32_T jp1j;
|
||||
int32_T c;
|
||||
int32_T ix;
|
||||
real32_T temp;
|
||||
int32_T k;
|
||||
real32_T absxk;
|
||||
real32_T t;
|
||||
y = 0.0F;
|
||||
if (n < 1) {
|
||||
} else if (n == 1) {
|
||||
y = (real32_T)fabs(x_data[ix0 - 1]);
|
||||
} else {
|
||||
scale = 0.0F;
|
||||
firstNonZero = TRUE;
|
||||
kend = (ix0 + n) - 1;
|
||||
for (k = ix0 - 1; k + 1 <= kend; k++) {
|
||||
if (x_data[k] != 0.0F) {
|
||||
absxk = (real32_T)fabs(x_data[k]);
|
||||
if (firstNonZero) {
|
||||
scale = absxk;
|
||||
y = 1.0F;
|
||||
firstNonZero = FALSE;
|
||||
} else if (scale < absxk) {
|
||||
t = scale / absxk;
|
||||
y = 1.0F + y * t * t;
|
||||
scale = absxk;
|
||||
} else {
|
||||
t = absxk / scale;
|
||||
y += t * t;
|
||||
}
|
||||
real32_T s;
|
||||
int32_T loop_ub;
|
||||
real32_T Y[108];
|
||||
for (jy = 0; jy < 9; jy++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
b_A[iy + 9 * jy] = B[jy + 9 * iy];
|
||||
}
|
||||
|
||||
ipiv[jy] = (int8_T)(1 + jy);
|
||||
}
|
||||
|
||||
for (j = 0; j < 8; j++) {
|
||||
mmj = -j;
|
||||
jj = j * 10;
|
||||
jp1j = jj + 1;
|
||||
c = mmj + 9;
|
||||
jy = 0;
|
||||
ix = jj;
|
||||
temp = (real32_T)fabsf(b_A[jj]);
|
||||
for (k = 2; k <= c; k++) {
|
||||
ix++;
|
||||
s = (real32_T)fabsf(b_A[ix]);
|
||||
if (s > temp) {
|
||||
jy = k - 1;
|
||||
temp = s;
|
||||
}
|
||||
}
|
||||
|
||||
y = scale * (real32_T)sqrt(y);
|
||||
}
|
||||
if ((real_T)b_A[jj + jy] != 0.0) {
|
||||
if (jy != 0) {
|
||||
ipiv[j] = (int8_T)((j + jy) + 1);
|
||||
ix = j;
|
||||
iy = j + jy;
|
||||
for (k = 0; k < 9; k++) {
|
||||
temp = b_A[ix];
|
||||
b_A[ix] = b_A[iy];
|
||||
b_A[iy] = temp;
|
||||
ix += 9;
|
||||
iy += 9;
|
||||
}
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
loop_ub = (jp1j + mmj) + 8;
|
||||
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
|
||||
b_A[iy] /= b_A[jj];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const
|
||||
real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81],
|
||||
int32_T y_sizes[2])
|
||||
{
|
||||
int32_T b_A_sizes[2];
|
||||
int32_T loop_ub;
|
||||
int32_T i3;
|
||||
int32_T b_loop_ub;
|
||||
int32_T i4;
|
||||
real32_T b_A_data[81];
|
||||
int32_T b_B_sizes[2];
|
||||
real32_T b_B_data[81];
|
||||
int8_T unnamed_idx_0;
|
||||
int32_T c_B_sizes[2];
|
||||
real32_T c_B_data[81];
|
||||
b_A_sizes[0] = B_sizes[1];
|
||||
b_A_sizes[1] = B_sizes[0];
|
||||
loop_ub = B_sizes[0] - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
b_loop_ub = B_sizes[1] - 1;
|
||||
for (i4 = 0; i4 <= b_loop_ub; i4++) {
|
||||
b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4];
|
||||
c = 8 - j;
|
||||
jy = jj + 9;
|
||||
for (iy = 1; iy <= c; iy++) {
|
||||
if ((real_T)b_A[jy] != 0.0) {
|
||||
temp = b_A[jy] * -1.0F;
|
||||
ix = jp1j;
|
||||
loop_ub = (mmj + jj) + 18;
|
||||
for (k = 10 + jj; k + 1 <= loop_ub; k++) {
|
||||
b_A[k] += b_A[ix] * temp;
|
||||
ix++;
|
||||
}
|
||||
}
|
||||
|
||||
jy += 9;
|
||||
jj += 9;
|
||||
}
|
||||
}
|
||||
|
||||
b_B_sizes[0] = A_sizes[1];
|
||||
b_B_sizes[1] = 9;
|
||||
for (i3 = 0; i3 < 9; i3++) {
|
||||
loop_ub = A_sizes[1] - 1;
|
||||
for (i4 = 0; i4 <= loop_ub; i4++) {
|
||||
b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4];
|
||||
for (jy = 0; jy < 12; jy++) {
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
Y[iy + 9 * jy] = A[jy + 12 * iy];
|
||||
}
|
||||
}
|
||||
|
||||
if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) {
|
||||
unnamed_idx_0 = (int8_T)b_A_sizes[1];
|
||||
b_B_sizes[0] = (int32_T)unnamed_idx_0;
|
||||
b_B_sizes[1] = 9;
|
||||
loop_ub = unnamed_idx_0 * 9 - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
b_B_data[i3] = 0.0F;
|
||||
for (iy = 0; iy < 9; iy++) {
|
||||
if (ipiv[iy] != iy + 1) {
|
||||
for (j = 0; j < 12; j++) {
|
||||
temp = Y[iy + 9 * j];
|
||||
Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
|
||||
Y[(ipiv[iy] + 9 * j) - 1] = temp;
|
||||
}
|
||||
}
|
||||
} else if (b_A_sizes[0] == b_A_sizes[1]) {
|
||||
eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes);
|
||||
} else {
|
||||
c_B_sizes[0] = b_B_sizes[0];
|
||||
c_B_sizes[1] = 9;
|
||||
loop_ub = b_B_sizes[0] * 9 - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
c_B_data[i3] = b_B_data[i3];
|
||||
}
|
||||
|
||||
eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes);
|
||||
}
|
||||
|
||||
y_sizes[0] = 9;
|
||||
y_sizes[1] = b_B_sizes[0];
|
||||
loop_ub = b_B_sizes[0] - 1;
|
||||
for (i3 = 0; i3 <= loop_ub; i3++) {
|
||||
for (i4 = 0; i4 < 9; i4++) {
|
||||
y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4];
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 0; k < 9; k++) {
|
||||
jy = 9 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
for (iy = k + 2; iy < 10; iy++) {
|
||||
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < 12; j++) {
|
||||
c = 9 * j;
|
||||
for (k = 8; k > -1; k += -1) {
|
||||
jy = 9 * k;
|
||||
if ((real_T)Y[k + c] != 0.0) {
|
||||
Y[k + c] /= b_A[k + jy];
|
||||
for (iy = 0; iy + 1 <= k; iy++) {
|
||||
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (jy = 0; jy < 9; jy++) {
|
||||
for (iy = 0; iy < 12; iy++) {
|
||||
y[iy + 12 * jy] = Y[jy + 9 * iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@ -29,6 +29,8 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
|
||||
extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
|
||||
extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
|
||||
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
|
||||
#endif
|
||||
/* End of code generation (mrdivide.h) */
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
84
apps/attitude_estimator_ekf/codegen/power.c
Executable file
84
apps/attitude_estimator_ekf/codegen/power.c
Executable file
@ -0,0 +1,84 @@
|
||||
/*
|
||||
* power.c
|
||||
*
|
||||
* Code generation for function 'power'
|
||||
*
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "power.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 f0;
|
||||
real32_T f1;
|
||||
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else {
|
||||
f0 = (real32_T)fabsf(u0);
|
||||
f1 = (real32_T)fabsf(u1);
|
||||
if (rtIsInfF(u1)) {
|
||||
if (f0 == 1.0F) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else if (f0 > 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 (f1 == 0.0F) {
|
||||
y = 1.0F;
|
||||
} else if (f1 == 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)sqrtf(u0);
|
||||
} else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else {
|
||||
y = (real32_T)powf(u0, u1);
|
||||
}
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void power(const real32_T a[12], real_T b, real32_T y[12])
|
||||
{
|
||||
int32_T k;
|
||||
for (k = 0; k < 12; k++) {
|
||||
y[k] = rt_powf_snf(a[k], (real32_T)b);
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (power.c) */
|
||||
@ -1,14 +1,14 @@
|
||||
/*
|
||||
* find.h
|
||||
* power.h
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
* Code generation for function 'power'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __FIND_H__
|
||||
#define __FIND_H__
|
||||
#ifndef __POWER_H__
|
||||
#define __POWER_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
@ -29,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
|
||||
extern void power(const real32_T a[12], real_T b, real32_T y[12]);
|
||||
#endif
|
||||
/* End of code generation (find.h) */
|
||||
/* End of code generation (power.h) */
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@ -949,7 +949,12 @@ int commander_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("commander", SCHED_PRIORITY_MAX - 50, 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
deamon_task = task_spawn("commander",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 50,
|
||||
4096,
|
||||
commander_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
@ -1007,8 +1012,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
memset(¤t_status, 0, sizeof(current_status));
|
||||
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
|
||||
current_status.flag_system_armed = false;
|
||||
/* neither manual nor offboard control commands have been received */
|
||||
current_status.offboard_control_signal_found_once = false;
|
||||
current_status.rc_signal_found_once = false;
|
||||
/* mark all signals lost as long as they haven't been found */
|
||||
current_status.rc_signal_lost = true;
|
||||
current_status.offboard_control_signal_lost = true;
|
||||
|
||||
/* advertise to ORB */
|
||||
stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status);
|
||||
|
||||
@ -91,7 +91,12 @@ int px4_deamon_app_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, px4_deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
deamon_task = task_spawn("deamon",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
4096,
|
||||
px4_deamon_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -60,6 +60,7 @@
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/geo/geo.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
@ -414,7 +415,12 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
deamon_task = task_spawn("fixedwing_control",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
4096,
|
||||
fixedwing_control_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -57,6 +57,8 @@
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
static bool thread_should_exit; /**< Deamon status flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
@ -140,7 +142,12 @@ int gps_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
deamon_task = task_spawn("gps",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
4096,
|
||||
gps_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -75,7 +75,9 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
#include "mavlink_log.h"
|
||||
@ -203,7 +205,7 @@ int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
uint64_t mavlink_missionlib_get_system_timestamp(void);
|
||||
|
||||
void handleMessage(mavlink_message_t *msg);
|
||||
static void mavlink_update_system();
|
||||
static void mavlink_update_system(void);
|
||||
|
||||
/**
|
||||
* Enable / disable Hardware in the Loop simulation mode.
|
||||
@ -1549,9 +1551,9 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
|
||||
return uart;
|
||||
}
|
||||
|
||||
void mavlink_update_system()
|
||||
void mavlink_update_system(void)
|
||||
{
|
||||
static initialized = false;
|
||||
static bool initialized = false;
|
||||
param_t param_system_id;
|
||||
param_t param_component_id;
|
||||
param_t param_system_type;
|
||||
@ -1706,9 +1708,9 @@ int mavlink_thread_main(int argc, char *argv[])
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
@ -1866,7 +1868,12 @@ int mavlink_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
mavlink_task = task_spawn("mavlink",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
6000,
|
||||
mavlink_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
@ -67,6 +67,7 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
#include "multirotor_rate_control.h"
|
||||
@ -350,7 +351,12 @@ int multirotor_att_control_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1+optioncount], "start")) {
|
||||
|
||||
thread_should_exit = false;
|
||||
mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
|
||||
mc_task = task_spawn("multirotor_att_control",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
2048,
|
||||
mc_thread_main,
|
||||
NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
@ -66,6 +66,8 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
class FMUServo : public device::CDev
|
||||
{
|
||||
public:
|
||||
@ -169,7 +171,12 @@ FMUServo::init()
|
||||
return ret;
|
||||
|
||||
/* start the IO interface task */
|
||||
_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
|
||||
_task = task_spawn("fmuservo",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
(main_t)&FMUServo::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_task < 0) {
|
||||
debug("task start failed: %d", errno);
|
||||
|
||||
@ -61,6 +61,8 @@
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
#include <systemlib/systemlib.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 */
|
||||
@ -120,7 +122,12 @@ int sdlog_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
deamon_task = task_spawn("sdlog",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_DEFAULT - 30,
|
||||
4096,
|
||||
sdlog_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -1175,11 +1175,12 @@ Sensors::start()
|
||||
ASSERT(_sensors_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_sensors_task = task_create("sensors_task",
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
6000, /* XXX may be excesssive */
|
||||
(main_t)&Sensors::task_main_trampoline,
|
||||
nullptr);
|
||||
_sensors_task = task_spawn("sensors_task",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
6000, /* XXX may be excesssive */
|
||||
(main_t)&Sensors::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_sensors_task < 0) {
|
||||
warn("task start failed");
|
||||
|
||||
@ -53,6 +53,9 @@
|
||||
#include <arch/board/up_hrt.h>
|
||||
#include <arch/board/drv_led.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
__EXPORT int led_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
@ -61,7 +64,7 @@ static bool thread_running = false; /**< Deamon status flag */
|
||||
static int led_task; /**< Handle of deamon task / thread */
|
||||
static int leds;
|
||||
|
||||
static int led_init()
|
||||
static int led_init(void)
|
||||
{
|
||||
leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
|
||||
|
||||
@ -76,7 +79,7 @@ static int led_init()
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void led_deinit()
|
||||
static void led_deinit(void)
|
||||
{
|
||||
close(leds);
|
||||
}
|
||||
@ -144,7 +147,12 @@ int led_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
led_task = task_create("led", SCHED_PRIORITY_MAX - 15, 4096, led_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
led_task = task_spawn("led",
|
||||
SCHED_RR,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
4096,
|
||||
led_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@ -68,10 +68,10 @@ const struct __multiport_info multiport_info = {
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
void kill_task(FAR _TCB *tcb, FAR void *arg);
|
||||
static void kill_task(FAR _TCB *tcb, FAR void *arg);
|
||||
|
||||
/****************************************************************************
|
||||
* user_start
|
||||
@ -116,11 +116,36 @@ void killall()
|
||||
sched_foreach(kill_task, NULL);
|
||||
}
|
||||
|
||||
void kill_task(FAR _TCB *tcb, FAR void *arg)
|
||||
static void kill_task(FAR _TCB *tcb, FAR void *arg)
|
||||
{
|
||||
kill(tcb->pid, SIGUSR1);
|
||||
}
|
||||
|
||||
int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
|
||||
{
|
||||
int pid;
|
||||
|
||||
sched_lock();
|
||||
|
||||
/* create the task */
|
||||
pid = task_create(name, priority, stack_size, entry, argv);
|
||||
|
||||
if (pid > 0) {
|
||||
|
||||
/* configure the scheduler */
|
||||
struct sched_param param;
|
||||
|
||||
param.sched_priority = priority;
|
||||
sched_setscheduler(pid, scheduler, ¶m);
|
||||
|
||||
/* XXX do any other private task accounting here */
|
||||
}
|
||||
|
||||
sched_unlock();
|
||||
|
||||
return pid;
|
||||
}
|
||||
|
||||
#define PX4_BOARD_ID_FMU (5)
|
||||
|
||||
int fmu_get_board_info(struct fmu_board_info_s *info)
|
||||
|
||||
@ -50,6 +50,14 @@ __EXPORT int reboot(void);
|
||||
/** Sends SIGUSR1 to all processes */
|
||||
__EXPORT void killall(void);
|
||||
|
||||
/** Starts a task and performs any specific accounting, scheduler setup, etc. */
|
||||
__EXPORT int task_spawn(const char *name,
|
||||
int priority,
|
||||
int scheduler,
|
||||
int stack_size,
|
||||
main_t entry,
|
||||
const char *argv[]);
|
||||
|
||||
enum MULT_PORTS {
|
||||
MULT_0_US2_RXTX = 0,
|
||||
MULT_1_US2_FLOW,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user