diff options
46 files changed, 1329 insertions, 1382 deletions
@@ -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 diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 2a771cac4..532dd6a25 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -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 # -# Fire up the AR.Drone controller. +multirotor_att_control start + # -# XXX this should be '<command> start'. +# Fire up the AR.Drone interface. # -ardrone_control -d /dev/ttyS1 -m attitude & +ardrone_interface start # -# Start looking for a GPS. -# -# XXX this should not need to be backgrounded +# Start logging # -#gps -d /dev/ttyS3 -m all & +#sdlog start # -# Start logging to microSD if we can +# Start GPS capture # -#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
\ No newline at end of file diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 7ebd37e75..296de721b 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -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 diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8ed6db664..74b32c4af 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -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); } diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 932b49f5c..a8f80fd4c 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -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 diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 867b484e1..46c1a6623 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -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)); - } - - 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); + // 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; + + // // 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++; diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 459dcc9b9..86d872fd2 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -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
*
*/
@@ -15,7 +15,7 @@ #include "eye.h"
#include "mrdivide.h"
#include "diag.h"
-#include "find.h"
+#include "power.h"
/* Type Definitions */
@@ -52,662 +52,696 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) }
/*
- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst)
+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
*/
-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])
+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])
{
- int32_T udpIndVect_sizes;
- real_T udpIndVect_data[9];
- real32_T R_temp[9];
+ real32_T a[12];
+ int32_T i;
+ real32_T b_a[12];
+ real32_T Q[144];
+ real32_T O[9];
real_T dv0[9];
- int32_T ib;
- int32_T i0;
- real32_T fv0[81];
- real32_T b_knownConst[27];
- real32_T fv1[9];
- real32_T c_knownConst[9];
- real_T dv1[9];
- real_T dv2[9];
- real32_T A_lin[81];
+ real32_T c_a[9];
+ real32_T d_a[9];
real32_T x_n_b[3];
- real32_T K_k_data[81];
+ real32_T z_n_b[3];
+ real32_T x_apriori[12];
+ real32_T y_n_b[3];
+ int32_T i0;
+ real32_T e_a[3];
+ real_T dv1[144];
+ real32_T A_lin[144];
+ real32_T b_A_lin[144];
int32_T i1;
- real32_T b_A_lin[81];
- static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
-
- real32_T P_apriori[81];
- int32_T ia;
- static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ real32_T y;
+ real32_T P_apriori[144];
+ real32_T R[81];
+ real32_T b_P_apriori[108];
+ static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T K_k[108];
+ static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
- int8_T H_k_data[81];
- int32_T accUpt;
- int32_T magUpt;
- real32_T y;
- real32_T y_k_data[9];
- int32_T P_apriori_sizes[2];
- int32_T H_k_sizes[2];
- int32_T K_k_sizes[2];
- real32_T b_y[9];
- real_T dv3[81];
- real32_T c_y;
- real32_T z_n_b[3];
- real32_T y_n_b[3];
-
- /* Extended Attitude Kalmanfilter */
+ real32_T fv0[81];
+ real32_T c_P_apriori[36];
+ static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 };
+
+ real32_T fv1[36];
+ static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T S_k[36];
+ real32_T d_P_apriori[72];
+ static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0 };
+
+ real32_T b_K_k[72];
+ static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0 };
+
+ real32_T b_r[6];
+ static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ real32_T fv2[6];
+ real32_T b_z[6];
+ real32_T b_y;
+
+ /* Extended Attitude Kalmanfilter */
/* */
- /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
- /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
- /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
/* */
- /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
/* */
- /* Example.... */
+ /* Example.... */
/* */
/* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
- /* %define the matrices */
- /* tpred=0.005; */
- /* */
- /* A=[ 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */
- /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */
- /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */
- /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */
- /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */
- /* */
- /* */
- /* b=[70, 0, 0; */
- /* 0, 70, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0; */
- /* 0, 0, 0]; */
- /* */
- /* */
- /* C=[1, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, 1, 0, 0, 0, 0, 0, 0, 0; */
- /* 0, 0, 1, 0, 0, 0, 0, 0, 0; */
- /* 0, 0, 0, 1, 0, 0, 0, 0, 0; */
- /* 0, 0, 0, 0, 1, 0, 0, 0, 0; */
- /* 0, 0, 0, 0, 0, 1, 0, 0, 0; */
- /* 0, 0, 0, 0, 0, 0, 1, 0, 0; */
- /* 0, 0, 0, 0, 0, 0, 0, 1, 0; */
- /* 0, 0, 0, 0, 0, 0, 0, 0, 1]; */
- /* D=[]; */
- /* */
- /* */
- /* sys=ss(A,b,C,D); */
- /* */
- /* sysd=c2d(sys,tpred); */
- /* */
- /* */
- /* F=sysd.a; */
- /* */
- /* B=sysd.b; */
- /* */
- /* H=sysd.c; */
- /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */
- find(updVect, udpIndVect_data, &udpIndVect_sizes);
-
- /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */
- /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */
- /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */
- /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */
- /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */
- /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */
- /* process noise covariance matrix */
- /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */
- /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */
- /* observation matrix */
- /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */
- /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */
- /* compute A(t,w) */
- /* x_aposteriori_k[10,11,12] should be [p,q,r] */
- /* R_temp=[1,-r, q */
- /* r, 1, -p */
- /* -q, p, 1] */
- /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */
- /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */
- /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */
- R_temp[0] = 1.0F;
- R_temp[3] = -dt * x_aposteriori_k[2];
- R_temp[6] = dt * x_aposteriori_k[1];
- R_temp[1] = dt * x_aposteriori_k[2];
- R_temp[4] = 1.0F;
- R_temp[7] = -dt * x_aposteriori_k[0];
- R_temp[2] = -dt * x_aposteriori_k[1];
- R_temp[5] = dt * x_aposteriori_k[0];
- R_temp[8] = 1.0F;
-
- /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */
- /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */
- /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */
- /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */
- /* 'attitudeKalmanfilter:112' 0, 0, 0; */
- /* 'attitudeKalmanfilter:113' 0, 0, 0; */
- /* 'attitudeKalmanfilter:114' 0, 0, 0; */
- /* 'attitudeKalmanfilter:115' 0, 0, 0; */
- /* 'attitudeKalmanfilter:116' 0, 0, 0; */
- /* 'attitudeKalmanfilter:117' 0, 0, 0; */
- /* 'attitudeKalmanfilter:118' 0, 0, 0]; */
- /* %prediction step */
- /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */
- eye(dv0);
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * (ib + 3)] = 0.0F;
- }
+ /* coder.varsize('udpIndVect', [9,1], [1,0]) */
+ /* udpIndVect=find(updVect); */
+ /* process and measurement noise covariance matrix */
+ /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */
+ power(q, 2.0, a);
+ for (i = 0; i < 12; i++) {
+ b_a[i] = a[i] * dt;
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * (ib + 6)] = 0.0F;
- }
- }
+ diag(b_a, Q);
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * ib) + 3] = 0.0F;
- }
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */
+ /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */
+ /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */
+ /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */
+ /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */
+ /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */
+ /* 'attitudeKalmanfilter:54' wk =[wx; */
+ /* 'attitudeKalmanfilter:55' wy; */
+ /* 'attitudeKalmanfilter:56' wz]; */
+ /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */
+ /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+ O[0] = 0.0F;
+ O[1] = -x_aposteriori_k[2];
+ O[2] = x_aposteriori_k[1];
+ O[3] = x_aposteriori_k[2];
+ O[4] = 0.0F;
+ O[5] = -x_aposteriori_k[0];
+ O[6] = -x_aposteriori_k[1];
+ O[7] = x_aposteriori_k[0];
+ O[8] = 0.0F;
+
+ /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ c_a[i] = (real32_T)dv0[i] + O[i] * dt;
}
- for (ib = 0; ib < 3; ib++) {
+ /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ d_a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */
+ /* 'attitudeKalmanfilter:64' -zez,0,zex; */
+ /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */
+ /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */
+ /* 'attitudeKalmanfilter:67' -muz,0,mux; */
+ /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */
+ /* 'attitudeKalmanfilter:72' E=eye(3); */
+ /* 'attitudeKalmanfilter:73' Z=zeros(3); */
+ /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */
+ x_n_b[0] = x_aposteriori_k[6];
+ x_n_b[1] = x_aposteriori_k[7];
+ x_n_b[2] = x_aposteriori_k[8];
+ z_n_b[0] = x_aposteriori_k[9];
+ z_n_b[1] = x_aposteriori_k[10];
+ z_n_b[2] = x_aposteriori_k[11];
+ x_apriori[0] = x_aposteriori_k[0];
+ x_apriori[1] = x_aposteriori_k[1];
+ x_apriori[2] = x_aposteriori_k[2];
+ x_apriori[3] = x_aposteriori_k[3];
+ x_apriori[4] = x_aposteriori_k[4];
+ x_apriori[5] = x_aposteriori_k[5];
+ for (i = 0; i < 3; i++) {
+ y_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0];
+ y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];
}
- }
- for (ib = 0; ib < 3; ib++) {
+ e_a[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
+ e_a[i] += d_a[i + 3 * i0] * z_n_b[i0];
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * ib) + 6] = 0.0F;
- }
+ x_apriori[i + 6] = y_n_b[i];
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
- }
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 9] = e_a[i];
}
- b_knownConst[0] = knownConst[8] * dt;
- b_knownConst[9] = 0.0F;
- b_knownConst[18] = 0.0F;
- b_knownConst[1] = 0.0F;
- b_knownConst[10] = knownConst[9] * dt;
- b_knownConst[19] = 0.0F;
- for (ib = 0; ib < 3; ib++) {
+ /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */
+ /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */
+ /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */
+ /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */
+ /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0];
+ A_lin[i0 + 12 * i] = 0.0F;
}
- b_knownConst[2 + 9 * ib] = 0.0F;
- b_knownConst[3 + 9 * ib] = 0.0F;
- b_knownConst[4 + 9 * ib] = 0.0F;
- b_knownConst[5 + 9 * ib] = 0.0F;
- b_knownConst[6 + 9 * ib] = 0.0F;
- b_knownConst[7 + 9 * ib] = 0.0F;
- b_knownConst[8 + 9 * ib] = 0.0F;
- }
-
- for (ib = 0; ib < 9; ib++) {
- fv1[ib] = 0.0F;
- for (i0 = 0; i0 < 9; i0++) {
- fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0];
- }
-
- c_knownConst[ib] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0];
+ A_lin[(i0 + 12 * i) + 3] = 0.0F;
}
-
- x_aposteriori[ib] = fv1[ib] + c_knownConst[ib];
}
- /* linearization */
- /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */
- /* 'attitudeKalmanfilter:126' dt, 0, -dt; */
- /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */
- R_temp[0] = 0.0F;
- R_temp[3] = -dt;
- R_temp[6] = dt;
- R_temp[1] = dt;
- R_temp[4] = 0.0F;
- R_temp[7] = -dt;
- R_temp[2] = -dt;
- R_temp[5] = dt;
- R_temp[8] = 0.0F;
-
- /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */
- /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */
- eye(dv0);
- eye(dv1);
- eye(dv2);
- for (ib = 0; ib < 3; ib++) {
+ A_lin[6] = 0.0F;
+ A_lin[7] = x_aposteriori_k[8];
+ A_lin[8] = -x_aposteriori_k[7];
+ A_lin[18] = -x_aposteriori_k[8];
+ A_lin[19] = 0.0F;
+ A_lin[20] = x_aposteriori_k[6];
+ A_lin[30] = x_aposteriori_k[7];
+ A_lin[31] = -x_aposteriori_k[6];
+ A_lin[32] = 0.0F;
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];
+ A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
}
}
- for (ib = 0; ib < 3; ib++) {
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[i0 + 9 * (ib + 3)] = 0.0F;
+ A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
}
}
- for (ib = 0; ib < 3; ib++) {
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[i0 + 9 * (ib + 6)] = 0.0F;
+ A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
}
}
- for (ib = 0; ib < 3; ib++) {
+ A_lin[9] = 0.0F;
+ A_lin[10] = x_aposteriori_k[11];
+ A_lin[11] = -x_aposteriori_k[10];
+ A_lin[21] = -x_aposteriori_k[11];
+ A_lin[22] = 0.0F;
+ A_lin[23] = x_aposteriori_k[9];
+ A_lin[33] = x_aposteriori_k[7];
+ A_lin[34] = -x_aposteriori_k[9];
+ A_lin[35] = 0.0F;
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib];
+ A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
}
}
- for (ib = 0; ib < 3; ib++) {
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib];
+ A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
}
}
- for (ib = 0; ib < 3; ib++) {
+ for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
+ A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
}
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
+ dt;
}
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib];
- }
- }
-
- /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
- x_n_b[0] = knownConst[0];
- x_n_b[1] = knownConst[0];
- x_n_b[2] = knownConst[1];
- diag(x_n_b, R_temp);
- for (ib = 0; ib < 9; ib++) {
- for (i0 = 0; i0 < 9; i0++) {
- K_k_data[ib + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 *
+ /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
i0];
}
}
-
- for (i0 = 0; i0 < 9; i0++) {
- b_A_lin[ib + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1];
- }
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib];
- }
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[i0 + 9 * (ib + 3)] = 0.0F;
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[i0 + 9 * (ib + 6)] = 0.0F;
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * ib) + 3] = 0.0F;
- }
- }
-
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] *
- knownConst[3];
- }
- }
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
+ P_apriori[i + 12 * i0] = y + Q[i + 12 * i0];
}
}
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * ib) + 6] = 0.0F;
- }
- }
+ /* %update */
+ /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
+ /* 'attitudeKalmanfilter:93' R=diag(r); */
+ b_diag(r, R);
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */
+ /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */
+ /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ b_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1
+ + 12 * i0];
+ }
+ }
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] *
- knownConst[2];
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
}
- }
- for (ib = 0; ib < 9; ib++) {
- for (i0 = 0; i0 < 9; i0++) {
- P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib];
- }
- }
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
+ }
- /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */
- if (!(udpIndVect_sizes == 0) == 1) {
- /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */
- for (ib = 0; ib < 9; ib++) {
- ia = udpIndVect_sizes - 1;
- for (i0 = 0; i0 <= ia; i0++) {
- H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T)
- udpIndVect_data[i0] + 9 * ib) - 1];
+ fv0[i + 9 * i0] = y + R[i + 9 * i0];
}
}
- /* %update step */
- /* 'attitudeKalmanfilter:140' accUpt=1; */
- accUpt = 1;
+ mrdivide(b_P_apriori, fv0, K_k);
- /* 'attitudeKalmanfilter:141' magUpt=1; */
- magUpt = 1;
-
- /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */
- ia = udpIndVect_sizes - 1;
- for (ib = 0; ib <= ia; ib++) {
+ /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
y = 0.0F;
- for (i0 = 0; i0 < 9; i0++) {
- y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0];
+ for (i0 = 0; i0 < 12; i0++) {
+ y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
}
- y_k_data[ib] = z_k_data[ib] - y;
+ c_a[i] = z[i] - y;
}
- /* 'attitudeKalmanfilter:143' if updVect(4)==1 */
- if (updVect[3] == 1) {
- /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */
- for (ib = 0; ib < 3; ib++) {
- x_n_b[ib] = z_k_data[ib + 3];
+ for (i = 0; i < 12; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ y += K_k[i + 12 * i0] * c_a[i0];
}
- if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) {
- /* 'attitudeKalmanfilter:145' accUpt=10000; */
- accUpt = 10000;
- }
+ x_aposteriori[i] = x_apriori[i] + y;
}
- /* 'attitudeKalmanfilter:149' if updVect(7)==1 */
- if (updVect[6] == 1) {
- /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */
- for (ib = 0; ib < 3; ib++) {
- x_n_b[ib] = z_k_data[ib + 6];
- }
+ /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
+ }
- if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) {
- /* 'attitudeKalmanfilter:152' magUpt=10000; */
- magUpt = 10000;
+ Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
}
}
- /* measurement noise covariance matrix */
- /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */
- /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */
- /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */
- /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */
- P_apriori_sizes[0] = 9;
- P_apriori_sizes[1] = udpIndVect_sizes;
- for (ib = 0; ib < 9; ib++) {
- ia = udpIndVect_sizes - 1;
- for (i0 = 0; i0 <= ia; i0++) {
- b_A_lin[ib + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0
- + udpIndVect_sizes * i1];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];
}
}
}
-
- ia = udpIndVect_sizes - 1;
- for (ib = 0; ib <= ia; ib++) {
- for (i0 = 0; i0 < 9; i0++) {
- K_k_data[ib + udpIndVect_sizes * i0] = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib +
- udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0];
+ } else {
+ /* 'attitudeKalmanfilter:108' else */
+ /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
+ /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */
+ c_diag(*(real32_T (*)[3])&r[0], O);
+
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */
+ /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
+ /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ c_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv2[i1 + 12 * i0];
+ }
}
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5];
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ fv1[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * (ib + 3)] = 0.0F;
- }
- }
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
+ }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[i0 + 9 * (ib + 6)] = 0.0F;
+ c_a[i + 3 * i0] = y + O[i + 3 * i0];
+ }
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * ib) + 3] = 0.0F;
- }
- }
+ b_mrdivide(c_P_apriori, c_a, S_k);
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6]
- * (real32_T)accUpt;
- }
- }
+ /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];
+ }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
+ x_n_b[i] = z[i] - y;
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * ib) + 6] = 0.0F;
+ for (i = 0; i < 12; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ y += S_k[i + 12 * i0] * x_n_b[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + y;
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
+ /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
+ }
+
+ Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ }
}
- }
- for (ib = 0; ib < 3; ib++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7]
- * (real32_T)magUpt;
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
}
- }
+ } else {
+ /* 'attitudeKalmanfilter:123' else */
+ /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
+ {
+ /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */
+ d_diag(*(real32_T (*)[6])&r[0], S_k);
+
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */
+ /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */
+ /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv4[i1 + 12 * i0];
+ }
+ }
+ }
- H_k_sizes[0] = udpIndVect_sizes;
- H_k_sizes[1] = udpIndVect_sizes;
- ia = udpIndVect_sizes - 1;
- for (ib = 0; ib <= ia; ib++) {
- accUpt = udpIndVect_sizes - 1;
- for (i0 = 0; i0 <= accUpt; i0++) {
- y = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 +
- udpIndVect_sizes * i1];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
}
- A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] +
- 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1];
- }
- }
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
+ }
+
+ fv1[i + 6 * i0] = y + S_k[i + 6 * i0];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, fv1, b_K_k);
- mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes);
+ /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 6; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];
+ }
- /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */
- if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) {
- for (ib = 0; ib < 9; ib++) {
- b_y[ib] = 0.0F;
- ia = udpIndVect_sizes - 1;
- for (i0 = 0; i0 <= ia; i0++) {
- b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0];
+ b_r[i] = z[i] - y;
}
- }
- } else {
- for (accUpt = 0; accUpt < 9; accUpt++) {
- b_y[accUpt] = 0.0F;
- }
- magUpt = -1;
- for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) {
- if ((real_T)y_k_data[ib] != 0.0) {
- ia = magUpt;
- for (accUpt = 0; accUpt < 9; accUpt++) {
- ia++;
- b_y[accUpt] += y_k_data[ib] * K_k_data[ia];
+ for (i = 0; i < 12; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ y += b_K_k[i + 12 * i0] * b_r[i0];
}
+
+ x_aposteriori[i] = x_apriori[i] + y;
}
- magUpt += 9;
- }
- }
+ /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
+ }
- for (ib = 0; ib < 9; ib++) {
- x_aposteriori[ib] += b_y[ib];
- }
+ Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ }
+ }
- /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */
- b_eye(dv3);
- for (ib = 0; ib < 9; ib++) {
- for (i0 = 0; i0 < 9; i0++) {
- y = 0.0F;
- ia = K_k_sizes[1] - 1;
- for (i1 = 0; i1 <= ia; i1++) {
- y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 +
- udpIndVect_sizes * i0];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
}
+ } else {
+ /* 'attitudeKalmanfilter:139' else */
+ /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
+ {
+ /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */
+ /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+ }
- fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y;
- }
- }
+ for (i = 0; i < 3; i++) {
+ b_r[i << 1] = r[i];
+ b_r[1 + (i << 1)] = r[6 + i];
+ }
- for (ib = 0; ib < 9; ib++) {
- for (i0 = 0; i0 < 9; i0++) {
- P_aposteriori[ib + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];
+ }
+
+ S_k[i + 6 * i0] = y + b_r[3 * (i + i0)];
+ }
+ }
+
+ /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv7[i1 + 12 * i0];
+ }
+ }
+ }
+
+ c_mrdivide(d_P_apriori, S_k, b_K_k);
+
+ /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ b_r[i] = z[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ b_r[i + 3] = z[i + 6];
+ }
+
+ for (i = 0; i < 6; i++) {
+ fv2[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
+ }
+
+ b_z[i] = b_r[i] - fv2[i];
+ }
+
+ for (i = 0; i < 12; i++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ y += b_K_k[i + 12 * i0] * b_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + y;
+ }
+
+ /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
+ }
+
+ Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:155' else */
+ /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */
+ for (i = 0; i < 12; i++) {
+ x_aposteriori[i] = x_apriori[i];
+ }
+
+ /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */
+ memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof
+ (real32_T));
}
}
}
- } else {
- /* 'attitudeKalmanfilter:167' else */
- /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */
- /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */
- memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof
- (real32_T));
}
- /* %% euler anglels extraction */
- /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */
- y = norm(*(real32_T (*)[3])&x_aposteriori[3]);
+ /* % euler anglels extraction */
+ /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
+ y = norm(*(real32_T (*)[3])&x_aposteriori[6]);
- /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
- c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]);
+ /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
+ b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]);
- /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */
- for (accUpt = 0; accUpt < 3; accUpt++) {
- z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y;
- x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y;
+ /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */
+ for (i = 0; i < 3; i++) {
+ z_n_b[i] = -x_aposteriori[i + 6] / y;
+ x_n_b[i] = x_aposteriori[i + 9] / b_y;
}
cross(z_n_b, x_n_b, y_n_b);
- /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */
+ /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */
y = norm(y_n_b);
- for (ib = 0; ib < 3; ib++) {
- y_n_b[ib] /= y;
+ for (i = 0; i < 3; i++) {
+ y_n_b[i] /= y;
}
- /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */
+ /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */
cross(y_n_b, z_n_b, x_n_b);
- /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */
+ /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */
y = norm(x_n_b);
- for (ib = 0; ib < 3; ib++) {
- /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
- Rot_matrix[ib] = x_n_b[ib] / y;
- Rot_matrix[3 + ib] = y_n_b[ib];
- Rot_matrix[6 + ib] = z_n_b[ib];
+ for (i = 0; i < 3; i++) {
+ /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+ Rot_matrix[i] = x_n_b[i] / y;
+ Rot_matrix[3 + i] = y_n_b[i];
+ Rot_matrix[6 + i] = z_n_b[i];
}
- /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
- /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */
- /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
- /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */
+ /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 56f02b4c8..f8f99fa5a 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index b72256a09..689bc49e9 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index efb465b25..dcce3cd72 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index d0bf625f0..39f8f648c 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index 1ad84575f..ea7b9e03e 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index bd83cc168..6d5704a7a 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index c88869453..27da4b6b9 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 92e3a884d..8ef2c475c 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c index 522f6e285..81626589d 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/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
*
*/
@@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index bb92fb242..10cea81b2 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e67071dce..2db070e6f 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -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;
}
}
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index c07a1bc97..027db1c06 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.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
*
*/
@@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c deleted file mode 100755 index 8f05ef146..000000000 --- a/apps/attitude_estimator_ekf/codegen/find.c +++ /dev/null @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index d098162d5..ce29e42cd 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.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];
}
}
- 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 (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 = 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 = 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];
}
}
}
}
-}
-/*
- *
- */
-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)
-{
- 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 (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 (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;
+ for (jy = 0; jy < 6; jy++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * jy] = Y[jy + 6 * iy];
+ }
+ }
}
/*
*
*/
-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])
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
- 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 jy;
+ int32_T iy;
+ 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 smax;
+ real32_T temp;
+ int32_T k;
real32_T s;
- 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;
+ 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];
}
- loop_ub = A_sizes[1] - 1;
- for (k = 0; k <= loop_ub; k++) {
- jpvt_data[k] = tmp_data[k];
- }
+ ipiv[jy] = (int8_T)(1 + jy);
}
- 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;
- }
- }
- }
+ 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;
}
+ }
- 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++;
+ 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;
}
-
- 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;
+ loop_ub = (jp1j + mmj) + 8;
+ for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
+ b_A[iy] /= b_A[jj];
}
+ }
- 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];
- }
- }
+ 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++;
}
-
- 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);
- }
- }
- }
+ jy += 9;
+ jj += 9;
}
}
- 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 (jy = 0; jy < 12; jy++) {
+ for (iy = 0; iy < 9; iy++) {
+ Y[iy + 9 * jy] = A[jy + 12 * iy];
}
}
- 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];
+ 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;
}
}
}
-}
-/*
- *
- */
-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;
- 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;
+ 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];
}
}
}
-
- y = scale * (real32_T)sqrt(y);
- }
-
- return y;
-}
-
-/*
- *
- */
-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];
- }
}
- 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];
- }
- }
-
- 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;
- }
- } 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];
+ 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];
+ }
+ }
}
-
- 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 (jy = 0; jy < 9; jy++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * jy] = Y[jy + 9 * iy];
}
}
}
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e807afcc8..b80f34357 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index fa07e1a90..341c93022 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index 63294717f..0f58dbe69 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c new file mode 100755 index 000000000..8672c7a85 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/power.c @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/power.h index ca525c600..a60f1bb25 100755 --- a/apps/attitude_estimator_ekf/codegen/find.h +++ b/apps/attitude_estimator_ekf/codegen/power.h @@ -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) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index f1bb71c87..53686acc9 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 6d32d51b5..5ac1dc794 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 50581f34d..1e1876b80 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 12d8734f5..5f1c81f67 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 419edf01c..5f65f6de9 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 42ff21d39..2c687d7a1 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 60128156e..d2ebd0806 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -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
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index 2709915e9..b487c8b38 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -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
*
*/
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 960c5d383..77e4da850 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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); diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c index 053163789..6068df5a3 100644 --- a/apps/examples/px4_deamon_app/px4_deamon_app.c +++ b/apps/examples/px4_deamon_app/px4_deamon_app.c @@ -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); } diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index beabc1142..81ec4ca0b 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -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); } diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 976beeaab..8304c72e1 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -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); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index cf6fb077f..b0cf01fd4 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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. @@ -1552,9 +1554,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; @@ -1709,9 +1711,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); @@ -1869,7 +1871,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); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ebd9911a3..9acdc6fd3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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); } diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 48d338c99..eeef2f914 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -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); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f1c1f9a23..f4af17597 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -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); } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index a68f6166c..b9de42f76 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1173,11 +1173,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"); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c index a6d73210a..da0d99964 100644 --- a/apps/systemcmds/led/led.c +++ b/apps/systemcmds/led/led.c @@ -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); } diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 4c7aae83e..3d46a17a8 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -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) diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h index add47f440..da1524435 100644 --- a/apps/systemlib/systemlib.h +++ b/apps/systemlib/systemlib.h @@ -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, |