From 1cf9f72f628c5dbdf487e464699245cab61c1750 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 3 Nov 2013 12:40:13 -0500 Subject: Add data manager module and fence support to navigator - Add function to geo.c to determine if global position is inside fence - Add navigator support/commands for maintaining fence coords. - Add data manager module to support persistence fence storage. Can store other data, but only used for fence at this time. - Add unit tests for data manager --- src/systemcmds/tests/module.mk | 3 + src/systemcmds/tests/test_dataman.c | 179 ++++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 184 insertions(+) create mode 100644 src/systemcmds/tests/test_dataman.c (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6729ce4ae..53c0e6b4f 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,9 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_dataman.c \ tests_file.c \ tests_main.c \ tests_param.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c new file mode 100644 index 000000000..e33c5aceb --- /dev/null +++ b/src/systemcmds/tests/test_dataman.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * px4/sensors/test_dataman.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + + +#include "tests.h" + +#include "dataman/dataman.h" + +static sem_t *sems; + +static int +task_main(int argc, char *argv[]) +{ + char buffer[DM_MAX_DATA_SIZE]; + hrt_abstime wstart, wend, rstart, rend; + + warnx("Starting dataman test task %s", argv[1]); + /* try to read an invalid item */ + int my_id = atoi(argv[1]); + /* try to read an invalid item */ + if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid item failed", my_id); + goto fail; + } + /* try to read an invalid index */ + if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid index failed", my_id); + goto fail; + } + srand(hrt_absolute_time() ^ my_id); + unsigned hit = 0, miss = 0; + wstart = hrt_absolute_time(); + for (unsigned i = 0; i < 256; i++) { + memset(buffer, my_id, sizeof(buffer)); + buffer[1] = i; + unsigned hash = i ^ my_id; + unsigned len = (hash & 63) + 2; + if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) { + warnx("%d write failed, index %d, length %d", my_id, hash, len); + goto fail; + } + usleep(rand() & ((64 * 1024) - 1)); + } + rstart = hrt_absolute_time(); + wend = rstart; + + for (unsigned i = 0; i < 256; i++) { + unsigned hash = i ^ my_id; + unsigned len2, len = (hash & 63) + 2; + if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { + warnx("%d read failed length test, index %d", my_id, hash); + goto fail; + } + if (buffer[0] == my_id) { + hit++; + if (len2 != len) { + warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); + goto fail; + } + if (buffer[1] != i) { + warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); + goto fail; + } + } + else + miss++; + } + rend = hrt_absolute_time(); + warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", + my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); + sem_post(sems + my_id); + return 0; +fail: + warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", + my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); + sem_post(sems + my_id); + return -1; +} + +int test_dataman(int argc, char *argv[]) +{ + int i, num_tasks = 4; + char buffer[DM_MAX_DATA_SIZE]; + + if (argc > 1) + num_tasks = atoi(argv[1]); + + sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); + warnx("Running %d tasks", num_tasks); + for (i = 0; i < num_tasks; i++) { + int task; + char a[16]; + sprintf(a, "%d", i); + const char *av[2]; + av[0] = a; + av[1] = 0; + sem_init(sems + i, 1, 0); + /* start the task */ + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { + warn("task start failed"); + } + } + for (i = 0; i < num_tasks; i++) { + sem_wait(sems + i); + sem_destroy(sems + i); + } + free(sems); + dm_restart(DM_INIT_REASON_IN_FLIGHT); + for (i = 0; i < 256; i++) { + if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) + break; + } + if (i >= 256) { + warnx("Restart in-flight failed"); + return -1; + + } + dm_restart(DM_INIT_REASON_POWER_ON); + for (i = 0; i < 256; i++) { + if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { + warnx("Restart power-on failed"); + return -1; + } + } + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf..4a5eaa0c5 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -101,6 +101,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_dataman(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9eb9c632e..9b1cfb1fe 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, + {"dataman", test_dataman, OPT_NOALLTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From e034f5135ebabeb751ea775f2d79440cf74c8047 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Dec 2013 16:25:46 +0100 Subject: Dataman: Some minor fixes --- src/modules/dataman/dataman.c | 16 +++++++--------- src/modules/dataman/dataman.h | 5 ++--- src/systemcmds/tests/test_dataman.c | 23 +++++++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 05b77da20..dd3573d9a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -33,10 +33,8 @@ * ****************************************************************************/ /** - * @file navigator_main.c - * Implementation of the main navigation state machine. - * - * Handles missions, geo fencing and failsafe navigation behavior. + * @file dataman.c + * DATAMANAGER driver. */ #include @@ -113,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAY_POINTS_MAX, + DM_KEY_WAYPOINTS_MAX, }; /* Table of offset for index 0 of each item type */ @@ -138,7 +136,7 @@ static work_q_t g_work_q; sem_t g_work_queued_sema; sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, sensor task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; @@ -266,11 +264,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); - if (offset < 0) + if (offset < 0) return -1; /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) return -1; /* Write out the data, prefixed with length and persistence level */ @@ -456,7 +454,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return -1; /* Will return with queues locked */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) return -1; /* queues unlocked on failure */ work->func = dm_write_func; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 41ddfaf61..9e1f789ad 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -41,7 +41,6 @@ #include #include -#include #ifdef __cplusplus extern "C" { @@ -51,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAY_POINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +58,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT, + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index e33c5aceb..7de87b476 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -83,12 +83,15 @@ task_main(int argc, char *argv[]) srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) { + + int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + warnx("ret: %d", ret); + if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } @@ -97,10 +100,10 @@ task_main(int argc, char *argv[]) rstart = hrt_absolute_time(); wend = rstart; - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -120,7 +123,7 @@ task_main(int argc, char *argv[]) } rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", - my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: @@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[]) } free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) break; } - if (i >= 256) { + if (i >= NUM_MISSIONS_SUPPORTED) { warnx("Restart in-flight failed"); return -1; } dm_restart(DM_INIT_REASON_POWER_ON); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } -- cgit v1.2.3 From 4d846b480c9118090fe60a887fb1eb0824b38f56 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 5 Dec 2013 16:24:11 +0100 Subject: fix typo in makefile (resulted from previous merge) --- src/systemcmds/tests/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index cccd1b8ec..c1f8074ea 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -25,9 +25,9 @@ SRCS = test_adc.c \ test_uart_send.c \ test_mixer.cpp \ test_dataman.c \ - tests_file.c \ + test_file.c \ tests_main.c \ - tests_param.c \ + test_param.c \ test_ppm_loopback.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From 6a624abd7c8fe9cf32b5f5a91bceeb93f730a0ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: Datamanager: Rename mavlink/offboard key --- src/modules/dataman/dataman.c | 2 +- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/waypoints.c | 6 +++--- src/systemcmds/tests/test_dataman.c | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index acd612d9e..874a47be7 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dab96eb9b..2a781405a 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +59,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7aad5038d..52a580d5b 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -701,7 +701,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -975,7 +975,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi size_t len = sizeof(struct mission_item_s); - if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; @@ -1117,7 +1117,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* prepare mission topic */ mission.count = 0; - if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 7de87b476..5b121e34e 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } -- cgit v1.2.3 From a83e3cd22276109301678c204e83050483200d6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 19:33:47 +0400 Subject: New mathlib, WIP --- makefiles/config_px4fmu-v1_test.mk | 50 ++++ src/lib/conversion/rotation.cpp | 15 +- src/lib/mathlib/math/Dcm.cpp | 174 ------------- src/lib/mathlib/math/Dcm.hpp | 108 -------- src/lib/mathlib/math/EulerAngles.cpp | 126 --------- src/lib/mathlib/math/EulerAngles.hpp | 74 ------ src/lib/mathlib/math/Matrix.cpp | 193 -------------- src/lib/mathlib/math/Matrix.hpp | 253 ++++++++++++++++-- src/lib/mathlib/math/Matrix3.cpp | 46 ++++ src/lib/mathlib/math/Matrix3.hpp | 348 +++++++++++++++++++++++++ src/lib/mathlib/math/Quaternion.cpp | 174 ------------- src/lib/mathlib/math/Quaternion.hpp | 83 ++---- src/lib/mathlib/math/Vector.cpp | 100 -------- src/lib/mathlib/math/Vector.hpp | 214 ++++++++++++++-- src/lib/mathlib/math/Vector2.hpp | 269 ++++++++++++++++++++ src/lib/mathlib/math/Vector2f.cpp | 103 -------- src/lib/mathlib/math/Vector2f.hpp | 79 ------ src/lib/mathlib/math/Vector3.cpp | 99 -------- src/lib/mathlib/math/Vector3.hpp | 271 +++++++++++++++++--- src/lib/mathlib/math/arm/Matrix.cpp | 40 --- src/lib/mathlib/math/arm/Matrix.hpp | 292 --------------------- src/lib/mathlib/math/arm/Vector.cpp | 40 --- src/lib/mathlib/math/arm/Vector.hpp | 236 ----------------- src/lib/mathlib/math/generic/Matrix.cpp | 40 --- src/lib/mathlib/math/generic/Matrix.hpp | 437 -------------------------------- src/lib/mathlib/math/generic/Vector.cpp | 40 --- src/lib/mathlib/math/generic/Vector.hpp | 245 ------------------ src/lib/mathlib/mathlib.h | 11 +- src/lib/mathlib/module.mk | 18 +- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_mathlib.cpp | 114 +++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 33 files changed, 1528 insertions(+), 2767 deletions(-) create mode 100644 makefiles/config_px4fmu-v1_test.mk delete mode 100644 src/lib/mathlib/math/Dcm.cpp delete mode 100644 src/lib/mathlib/math/Dcm.hpp delete mode 100644 src/lib/mathlib/math/EulerAngles.cpp delete mode 100644 src/lib/mathlib/math/EulerAngles.hpp delete mode 100644 src/lib/mathlib/math/Matrix.cpp create mode 100644 src/lib/mathlib/math/Matrix3.cpp create mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Quaternion.cpp delete mode 100644 src/lib/mathlib/math/Vector.cpp create mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector2f.cpp delete mode 100644 src/lib/mathlib/math/Vector2f.hpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.cpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.cpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.cpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp create mode 100644 src/systemcmds/tests/test_mathlib.cpp (limited to 'src/systemcmds/tests') diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk new file mode 100644 index 000000000..42a35c5dd --- /dev/null +++ b/makefiles/config_px4fmu-v1_test.mk @@ -0,0 +1,50 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4fmu-v1 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB +MODULES += modules/systemlib/mixer + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2..bac594ab9 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081..000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3a..000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb..000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7..000000000 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math - diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..f05360a19 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +34,241 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix3.hpp * - * matrix code + * 3x3 Matrix */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include "../CMSIS/Include/arm_math.h" namespace math { + +template class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +// MxN matrix with float elements +template +class MatrixBase { +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + //arm_mat_init_f32(&arm_mat, M, N, data); + arm_mat = {M, N, &data[0][0]}; + } + + /** + * access by index + */ + inline float &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + inline const float &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + unsigned int getRows() { + return M; + } + + unsigned int getCols() { + return N; + } + + /** + * test for equality + */ + bool operator ==(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * set to value + */ + const MatrixBase &operator =(const MatrixBase &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + MatrixBase operator -(void) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + MatrixBase operator +(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + MatrixBase &operator +=(const MatrixBase &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + MatrixBase operator -(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + MatrixBase &operator -=(const MatrixBase &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + MatrixBase operator *(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] * num; + return res; + } + + MatrixBase &operator *=(const float num) { + return *this = *this * num; + } + + MatrixBase operator /(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + return res; + } + + MatrixBase &operator /=(const float num) { + return *this = *this / num; + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + for (unsigned int i = 0; i < M; i++) + data[i][i] = 1; + } + + void dump(void) { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + printf("\n"); + } + } +}; + +template +class Matrix : public MatrixBase { +public: + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector operator *(const Vector &v) const { + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } + */ +}; + +template <> +class Matrix<3, 3> : public MatrixBase<3, 3> { +public: + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res; + res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); + res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); + res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); + return res; + } + */ +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp new file mode 100644 index 000000000..3f1259dc4 --- /dev/null +++ b/src/lib/mathlib/math/Matrix3.cpp @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix3.cpp + * + * 3x3 Matrix + */ + +#include "Matrix3.hpp" + +namespace math +{ +} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp new file mode 100644 index 000000000..3236cd3d1 --- /dev/null +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -0,0 +1,348 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix3.hpp + * + * 3x3 Matrix + */ + +#ifndef MATRIX3_HPP +#define MATRIX3_HPP + +#include "Vector3.hpp" +#include "../CMSIS/Include/arm_math.h" + +namespace math +{ + +// 3x3 matrix with elements of type T +template +class Matrix3 { +public: + /** + * matrix data[row][col] + */ + T data[3][3]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + Matrix3() { + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T d[3][3]) { + memcpy(data, d, sizeof(data)); + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + data[0][0] = ax; + data[0][1] = ay; + data[0][2] = az; + data[1][0] = bx; + data[1][1] = by; + data[1][2] = bz; + data[2][0] = cx; + data[2][1] = cy; + data[2][2] = cz; + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * casting from a vector3f to a matrix is the tilde operator + */ + Matrix3(const Vector3 &v) { + arm_mat = {3, 3, &data[0][0]}; + data[0][0] = 0; + data[0][1] = -v.z; + data[0][2] = v.y; + data[1][0] = v.z; + data[1][1] = 0; + data[1][2] = -v.x; + data[2][0] = -v.y; + data[2][1] = v.x; + data[2][2] = 0; + } + + /** + * access by index + */ + inline T &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access to elements by index + */ + inline const T &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + /** + * test for equality + */ + bool operator ==(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * negation + */ + Matrix3 operator -(void) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + Matrix3 operator +(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + Matrix3 &operator +=(const Matrix3 &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + Matrix3 operator -(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + Matrix3 &operator -=(const Matrix3 &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + Matrix3 operator *(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] * num; + return res; + } + + Matrix3 &operator *=(const T num) { + return *this = *this * num; + } + + Matrix3 operator /(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] / num; + return res; + } + + Matrix3 &operator /=(const T num) { + return *this = *this / num; + } + + /** + * multiplication by a vector + */ + Vector3 operator *(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, + data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, + data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); + } + + /** + * multiplication of transpose by a vector + */ + Vector3 mul_transpose(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, + data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, + data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); + } + + /** + * multiplication by another matrix + */ + Matrix3 operator *(const Matrix3 &m) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) + Matrix3 res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), + data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), + data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), + data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), + data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), + data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), + data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), + data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), + data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); +#endif + } + + Matrix3 &operator *=(const Matrix3 &m) { + return *this = *this * m; + } + + /** + * transpose the matrix + */ + Matrix3 transposed(void) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float + Matrix3 res; + arm_mat_trans_f32(&arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0], data[1][0], data[2][0], + data[0][1], data[1][1], data[2][1], + data[0][2], data[1][2], data[2][2]); +#endif + } + + /** + * inverse the matrix + */ + Matrix3 inversed(void) const { + Matrix3 res; + arm_mat_inverse_f32(&arm_mat, &res.arm_mat); + return res; + } + + /** + * zero the matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + data[0][0] = 1; + data[1][1] = 1; + data[2][2] = 1; + } + + /** + * check if any elements are NAN + */ + bool is_nan(void) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (isnan(data[i][j])) + return true; + return false; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(T roll, T pitch, T yaw) { + T cp = (T)cosf(pitch); + T sp = (T)sinf(pitch); + T sr = (T)sinf(roll); + T cr = (T)cosf(roll); + T sy = (T)sinf(yaw); + T cy = (T)cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + // create eulers from a rotation matrix + //void to_euler(float *roll, float *pitch, float *yaw); + + // apply an additional rotation from a body frame gyro vector + // to a rotation matrix. + //void rotate(const Vector3 &g); +}; + +typedef Matrix3 Matrix3f; +} + +#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca6..000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d33..64b980ad8 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +36,35 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP -#include "Vector.hpp" -#include "Matrix.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector -{ +class Quaternion { public: + float real; + Vector3 imag; /** - * default ctor - */ - Quaternion(); - - /** - * ctor from floats - */ - Quaternion(float a, float b, float c, float d); - - /** - * ctor from data - */ - Quaternion(const float *data); - - /** - * ctor from Vector - */ - Quaternion(const Vector &v); - - /** - * ctor from EulerAngles + * trivial ctor */ - Quaternion(const EulerAngles &euler); + Quaternion() { + } /** - * ctor from Dcm + * setting ctor */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); - - /** - * derivative - */ - Vector derivative(const Vector &w); - - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + } }; +} -int __EXPORT quaternionTest(); -} // math - +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396..000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..adb2293e1 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +34,204 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Generic Vector */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template +class Vector { +public: + float data[N]; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector() { + arm_col = {N, 1, &data[0]}; + } + + /** + * setting ctor + */ + Vector(const float *d) { + memcpy(data, d, sizeof(data)); + arm_col = {N, 1, &data[0]}; + } + + /** + * access to elements by index + */ + inline float &operator ()(unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + inline const float &operator ()(unsigned int i) const { + return data[i]; + } + + /** + * test for equality + */ + bool operator ==(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return true; + return false; + } + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(data, v.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + const Vector operator -(void) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = -data[i]; + return res; + } + + /** + * addition + */ + const Vector operator +(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] + v(i); + return res; + } + + /** + * subtraction + */ + const Vector operator -(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] - v(i); + return res; + } + + /** + * uniform scaling + */ + const Vector operator *(const float num) const { + Vector temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector operator /(const float num) const { + Vector temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector &operator +=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v(i); + return *this; + } + + /** + * subtraction + */ + const Vector &operator -=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v(i); + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *this; + } + + /** + * dot product + */ + float operator *(const Vector &v) const { + float res; + for (unsigned int i = 0; i < N; i++) + res += data[i] * v(i); + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return sqrtf(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector normalized() const { + return *this / length(); + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp new file mode 100644 index 000000000..501740614 --- /dev/null +++ b/src/lib/mathlib/math/Vector2.hpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector2.hpp + * + * 2D Vector + */ + +#ifndef VECTOR2_HPP +#define VECTOR2_HPP + +#include + +namespace math +{ + +template +class Vector2 { +public: + T x, y; + + /** + * trivial ctor + */ + Vector2() { + } + + /** + * setting ctor + */ + Vector2(const T x0, const T y0): x(x0), y(y0) { + } + + /** + * setter + */ + void set(const T x0, const T y0) { + x = x0; + y = y0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector2 &v) { + return (x == v.x && y == v.y); + } + + /** + * test for inequality + */ + bool operator !=(const Vector2 &v) { + return (x != v.x || y != v.y); + } + + /** + * set to value + */ + const Vector2 &operator =(const Vector2 &v) { + x = v.x; + y = v.y; + return *this; + } + + /** + * negation + */ + const Vector2 operator -(void) const { + return Vector2(-x, -y); + } + + /** + * addition + */ + const Vector2 operator +(const Vector2 &v) const { + return Vector2(x + v.x, y + v.y); + } + + /** + * subtraction + */ + const Vector2 operator -(const Vector2 &v) const { + return Vector2(x - v.x, y - v.y); + } + + /** + * uniform scaling + */ + const Vector2 operator *(const T num) const { + Vector2 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector2 operator /(const T num) const { + Vector2 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector2 &operator +=(const Vector2 &v) { + x += v.x; + y += v.y; + return *this; + } + + /** + * subtraction + */ + const Vector2 &operator -=(const Vector2 &v) { + x -= v.x; + y -= v.y; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator *=(const T num) { + x *= num; + y *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator /=(const T num) { + x /= num; + y /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector2 &v) const { + return x * v.x + y * v.y; + } + + /** + * cross product + */ + const float operator %(const Vector2 &v) const { + return x * v.y - y * v.x; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector2 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector2 &n) + { + Vector2 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector2 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector2 projected(const Vector2 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector2 &v1, const Vector2 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { + return acosf(v1 * v2); + } +}; + +typedef Vector2 Vector2f; +} + +#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817..000000000 --- a/src/lib/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81c..000000000 --- a/src/lib/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2f.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600e..000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 568d9669a..a0c3b9290 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,43 +36,252 @@ /** * @file Vector3.hpp * - * math 3 vector + * 3D Vector */ -#pragma once +#ifndef VECTOR3_HPP +#define VECTOR3_HPP -#include "Vector.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class __EXPORT Vector3 : - public Vector -{ +template +class Vector3 { public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ + T x, y, z; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector3() { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { + arm_col = {3, 1, &x}; + } + + /** + * setter + */ + void set(const T x0, const T y0, const T z0) { + x = x0; + y = y0; + z = z0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector3 &v) { + return (x == v.x && y == v.y && z == v.z); + } + + /** + * test for inequality + */ + bool operator !=(const Vector3 &v) { + return (x != v.x || y != v.y || z != v.z); + } + + /** + * set to value + */ + const Vector3 &operator =(const Vector3 &v) { + x = v.x; + y = v.y; + z = v.z; + return *this; + } + + /** + * negation + */ + const Vector3 operator -(void) const { + return Vector3(-x, -y, -z); + } + + /** + * addition + */ + const Vector3 operator +(const Vector3 &v) const { + return Vector3(x + v.x, y + v.y, z + v.z); + } + + /** + * subtraction + */ + const Vector3 operator -(const Vector3 &v) const { + return Vector3(x - v.x, y - v.y, z - v.z); + } + + /** + * uniform scaling + */ + const Vector3 operator *(const T num) const { + Vector3 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector3 operator /(const T num) const { + Vector3 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector3 &operator +=(const Vector3 &v) { + x += v.x; + y += v.y; + z += v.z; + return *this; + } + + /** + * subtraction + */ + const Vector3 &operator -=(const Vector3 &v) { + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator *=(const T num) { + x *= num; + y *= num; + z *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator /=(const T num) { + x /= num; + y /= num; + z /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector3 &v) const { + return x * v.x + y * v.y + z * v.z; + } + + /** + * cross product + */ + const Vector3 operator %(const Vector3 &v) const { + Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + return temp; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector3 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector3 &n) + { + Vector3 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector3 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector3 projected(const Vector3 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector3 &v1, const Vector3 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { + return acosf(v1 * v2); + } }; -int __EXPORT vector3Test(); -} // math +typedef Vector3 Vector3f; +} +#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02d..000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc15..000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc..d8e95b46b 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,12 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" #include "math/Vector.hpp" +#include "math/Vector2.hpp" #include "math/Vector3.hpp" -#include "math/Vector2f.hpp" +#include "math/Matrix.hpp" +#include "math/Matrix3.hpp" +#include "math/Quaternion.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +55,4 @@ #include "CMSIS/Include/arm_math.h" -#endif \ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a..50951c617 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,7 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ + math/Matrix3.cpp \ math/Limits.cpp # @@ -49,13 +43,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d3..20182c5d8 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 000000000..13e6dad51 --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_mathlib.cpp + * + * Mathlib test + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + Matrix3f m; + m.identity(); + Matrix3f m1; + Matrix<3,3> mq; + mq.identity(); + Matrix<3,3> mq1; + m1(0, 0) = 5.0; + Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); + Vector3f v1; + + unsigned int n = 60000; + + hrt_abstime t0, t1; + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + v1 = m * v; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mq1 = mq * mq; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); + mq1.dump(); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + Matrix<4,4> mn; + mn(0, 0) = 2.0f; + mn(1, 0) = 3.0f; + for (int i = 0; i < mn.getRows(); i++) { + for (int j = 0; j < mn.getCols(); j++) { + printf("%.3f ", mn(i, j)); + } + printf("\n"); + } + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88..e2105f8ee 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18..fd026d70e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 23:01:02 +0400 Subject: mathlib: fixes and improvements, WIP --- src/lib/mathlib/math/Matrix.hpp | 38 ++++++++++----- src/lib/mathlib/math/Matrix3.hpp | 12 ++++- src/lib/mathlib/math/Vector.hpp | 90 ++++++++++++++++++++++++++--------- src/systemcmds/tests/test_mathlib.cpp | 80 ++++++++++++++++++++----------- 4 files changed, 157 insertions(+), 63 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f05360a19..1849f58be 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -34,9 +34,9 @@ ****************************************************************************/ /** - * @file Matrix3.hpp + * @file Matrix.hpp * - * 3x3 Matrix + * Generic Matrix */ #ifndef MATRIX_HPP @@ -203,6 +203,24 @@ public: return res; } + /** + * transpose the matrix + */ + Matrix transposed(void) const { + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix inversed(void) const { + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + /** * setup the identity matrix */ @@ -224,6 +242,8 @@ public: template class Matrix : public MatrixBase { public: + using MatrixBase::operator *; + /** * set to value */ @@ -235,18 +255,18 @@ public: /** * multiplication by a vector */ - /* Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } - */ }; template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: + using MatrixBase<3, 3>::operator *; + /** * set to value */ @@ -258,15 +278,11 @@ public: /** * multiplication by a vector */ - /* Vector<3> operator *(const Vector<3> &v) const { - Vector<3> res; - res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); - res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); - res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); - return res; + return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); } - */ }; } diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp index 3236cd3d1..c7eb67ba7 100644 --- a/src/lib/mathlib/math/Matrix3.hpp +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -74,14 +74,15 @@ public: * setting ctor */ Matrix3(const T d[3][3]) { - memcpy(data, d, sizeof(data)); arm_mat = {3, 3, &data[0][0]}; + memcpy(data, d, sizeof(data)); } /** * setting ctor */ Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + arm_mat = {3, 3, &data[0][0]}; data[0][0] = ax; data[0][1] = ay; data[0][2] = az; @@ -91,7 +92,6 @@ public: data[2][0] = cx; data[2][1] = cy; data[2][2] = cz; - arm_mat = {3, 3, &data[0][0]}; } /** @@ -124,6 +124,14 @@ public: return data[row][col]; } + /** + * set to value + */ + const Matrix3 &operator =(const Matrix3 &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + /** * test for equality */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index adb2293e1..cd00058b5 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -49,7 +49,7 @@ namespace math { template -class Vector { +class VectorBase { public: float data[N]; arm_matrix_instance_f32 arm_col; @@ -57,15 +57,22 @@ public: /** * trivial ctor */ - Vector() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** * setting ctor */ - Vector(const float *d) { - memcpy(data, d, sizeof(data)); +// VectorBase(const float *d) { + // memcpy(data, d, sizeof(data)); + //arm_col = {N, 1, &data[0]}; + //} + + /** + * setting ctor + */ + VectorBase(const float d[]) : data(d) { arm_col = {N, 1, &data[0]}; } @@ -86,7 +93,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -96,7 +103,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -106,7 +113,7 @@ public: /** * set to value */ - const Vector &operator =(const Vector &v) { + const VectorBase &operator =(const VectorBase &v) { memcpy(data, v.data, sizeof(data)); return *this; } @@ -114,8 +121,8 @@ public: /** * negation */ - const Vector operator -(void) const { - Vector res; + const VectorBase operator -(void) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = -data[i]; return res; @@ -124,8 +131,8 @@ public: /** * addition */ - const Vector operator +(const Vector &v) const { - Vector res; + const VectorBase operator +(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] + v(i); return res; @@ -134,8 +141,8 @@ public: /** * subtraction */ - const Vector operator -(const Vector &v) const { - Vector res; + const VectorBase operator -(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] - v(i); return res; @@ -144,23 +151,23 @@ public: /** * uniform scaling */ - const Vector operator *(const float num) const { - Vector temp(*this); + const VectorBase operator *(const float num) const { + VectorBase temp(*this); return temp *= num; } /** * uniform scaling */ - const Vector operator /(const float num) const { - Vector temp(*this); + const VectorBase operator /(const float num) const { + VectorBase temp(*this); return temp /= num; } /** * addition */ - const Vector &operator +=(const Vector &v) { + const VectorBase &operator +=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); return *this; @@ -169,7 +176,7 @@ public: /** * subtraction */ - const Vector &operator -=(const Vector &v) { + const VectorBase &operator -=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); return *this; @@ -178,7 +185,7 @@ public: /** * uniform scaling */ - const Vector &operator *=(const float num) { + const VectorBase &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; return *this; @@ -187,7 +194,7 @@ public: /** * uniform scaling */ - const Vector &operator /=(const float num) { + const VectorBase &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; return *this; @@ -196,7 +203,7 @@ public: /** * dot product */ - float operator *(const Vector &v) const { + float operator *(const VectorBase &v) const { float res; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); @@ -227,11 +234,48 @@ public: /** * returns the normalized version of this vector */ - Vector normalized() const { + VectorBase normalized() const { return *this / length(); } }; +template +class Vector : public VectorBase { +public: + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class Vector<3> : public VectorBase<3> { +public: + Vector<3>() { + arm_col = {3, 1, &this->data[0]}; + } + + Vector<3>(const float x, const float y, const float z) { + data[0] = x; + data[1] = y; + data[2] = z; + arm_col = {3, 1, &this->data[0]}; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + } #endif // VECTOR_HPP diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 13e6dad51..305ebbefa 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -58,15 +58,31 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix3f m; - m.identity(); - Matrix3f m1; - Matrix<3,3> mq; - mq.identity(); - Matrix<3,3> mq1; - m1(0, 0) = 5.0; - Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); - Vector3f v1; + Matrix<3,3> m3; + m3.identity(); + Matrix<4,4> m4; + m4.identity(); + Vector<3> v3; + v3(0) = 1.0f; + v3(1) = 2.0f; + v3(2) = 3.0f; + Vector<4> v4; + v4(0) = 1.0f; + v4(1) = 2.0f; + v4(2) = 3.0f; + v4(3) = 4.0f; + Vector<3> vres3; + Matrix<3,3> mres3; + Matrix<4,4> mres4; + + Matrix3f m3old; + m3old.identity(); + Vector3f v3old; + v3old.x = 1.0f; + v3old.y = 2.0f; + v3old.z = 3.0f; + Vector3f vres3old; + Matrix3f mres3old; unsigned int n = 60000; @@ -74,41 +90,51 @@ int test_mathlib(int argc, char *argv[]) t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - v1 = m * v; + vres3 = m3 * v3; } t1 = hrt_absolute_time(); - warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - mq1 = mq * mq; + vres3old = m3old * v3old; } t1 = hrt_absolute_time(); - warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); - mq1.dump(); + warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.transposed(); + mres3 = m3 * m3; } t1 = hrt_absolute_time(); - warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.inversed(); + mres3old = m3old * m3old; } t1 = hrt_absolute_time(); - warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); - - Matrix<4,4> mn; - mn(0, 0) = 2.0f; - mn(1, 0) = 3.0f; - for (int i = 0; i < mn.getRows(); i++) { - for (int j = 0; j < mn.getCols(); j++) { - printf("%.3f ", mn(i, j)); - } - printf("\n"); + warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres4 = m4 * m4; } + t1 = hrt_absolute_time(); + warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } -- cgit v1.2.3 From ba612c3ee8b88b9352e7cfa723997887dd736b76 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 14:10:25 +0400 Subject: mathlib fixes --- src/lib/conversion/rotation.cpp | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 ++- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Matrix.hpp | 172 +++++++--- src/lib/mathlib/math/Matrix3.cpp | 46 --- src/lib/mathlib/math/Matrix3.hpp | 356 --------------------- src/lib/mathlib/math/Quaternion.hpp | 79 ++++- src/lib/mathlib/math/Vector.hpp | 224 ++++++++++--- src/lib/mathlib/math/Vector2.hpp | 269 ---------------- src/lib/mathlib/math/Vector3.hpp | 287 ----------------- src/lib/mathlib/mathlib.h | 3 - src/lib/mathlib/module.mk | 1 - src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 92 +++--- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../commander/accelerometer_calibration.cpp | 14 +- src/modules/mavlink/mavlink_receiver.cpp | 11 +- src/modules/navigator/navigator_main.cpp | 16 +- src/modules/sensors/sensors.cpp | 14 +- src/systemcmds/tests/test_mathlib.cpp | 23 -- 22 files changed, 510 insertions(+), 1188 deletions(-) delete mode 100644 src/lib/mathlib/math/Matrix3.cpp delete mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector3.hpp (limited to 'src/systemcmds/tests') diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index bac594ab9..614877b18 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fc..0c56494c5 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959..fb7ed486d 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a92..5c0804a39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2..d7f350e5c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -29,7 +29,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -257,7 +257,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index f8f832ed7..68832a039 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -338,7 +338,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1849f58be..67214133a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -42,16 +42,19 @@ #ifndef MATRIX_HPP #define MATRIX_HPP +#include #include "../CMSIS/Include/arm_math.h" namespace math { -template +template class Matrix; +class Quaternion; + // MxN matrix with float elements -template +template class MatrixBase { public: /** @@ -69,10 +72,14 @@ public: * note that this ctor will not initialize elements */ MatrixBase() { - //arm_mat_init_f32(&arm_mat, M, N, data); arm_mat = {M, N, &data[0][0]}; } + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + /** * access by index */ @@ -98,10 +105,10 @@ public: /** * test for equality */ - bool operator ==(const MatrixBase &m) { + bool operator ==(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return false; return true; } @@ -109,10 +116,10 @@ public: /** * test for inequality */ - bool operator !=(const MatrixBase &m) { + bool operator !=(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return true; return false; } @@ -120,85 +127,97 @@ public: /** * set to value */ - const MatrixBase &operator =(const MatrixBase &m) { + const Matrix &operator =(const Matrix &m) { memcpy(data, m.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - MatrixBase operator -(void) const { - MatrixBase res; + Matrix operator -(void) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = -data[i][j]; + res.data[i][j] = -data[i][j]; return res; } /** * addition */ - MatrixBase operator +(const MatrixBase &m) const { - MatrixBase res; + Matrix operator +(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = data[i][j] + m(i, j); + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } - MatrixBase &operator +=(const MatrixBase &m) { - return *this = *this + m; + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *reinterpret_cast*>(this); } /** * subtraction */ - MatrixBase operator -(const MatrixBase &m) const { - MatrixBase res; + Matrix operator -(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] - m(i, j); + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } - MatrixBase &operator -=(const MatrixBase &m) { - return *this = *this - m; + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - MatrixBase operator *(const float num) const { - MatrixBase res; + Matrix operator *(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] * num; + res.data[i][j] = data[i][j] * num; return res; } - MatrixBase &operator *=(const float num) { - return *this = *this * num; + Matrix &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + return *reinterpret_cast*>(this); } - MatrixBase operator /(const float num) const { - MatrixBase res; + Matrix operator /(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) res[i][j] = data[i][j] / num; return res; } - MatrixBase &operator /=(const float num) { - return *this = *this / num; + Matrix &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + return *this; } /** * multiplication by another matrix */ template - Matrix operator *(const Matrix &m) const { - Matrix res; + Matrix operator *(const Matrix &m) const { + Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; } @@ -230,7 +249,7 @@ public: data[i][i] = 1; } - void dump(void) { + void print(void) { for (unsigned int i = 0; i < M; i++) { for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); @@ -244,6 +263,12 @@ class Matrix : public MatrixBase { public: using MatrixBase::operator *; + Matrix() : MatrixBase() { + } + + Matrix(const float *d) : MatrixBase(d) { + } + /** * set to value */ @@ -255,18 +280,28 @@ public: /** * multiplication by a vector */ - Vector operator *(const Vector &v) const { + Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } }; +} + +#include "Quaternion.hpp" +namespace math { template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; + Matrix() : MatrixBase<3, 3>() { + } + + Matrix(const float *d) : MatrixBase<3, 3>(d) { + } + /** * set to value */ @@ -279,9 +314,70 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given quaternion + */ + void from_quaternion(const Quaternion &q) { + float aSq = q.data[0] * q.data[0]; + float bSq = q.data[1] * q.data[1]; + float cSq = q.data[2] * q.data[2]; + float dSq = q.data[3] * q.data[3]; + data[0][0] = aSq + bSq - cSq - dSq; + data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); + data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); + data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); + data[1][1] = aSq - bSq + cSq - dSq; + data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); + data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); + data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); + data[2][2] = aSq - bSq - cSq + dSq; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } } }; diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp deleted file mode 100644 index 3f1259dc4..000000000 --- a/src/lib/mathlib/math/Matrix3.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix3.cpp - * - * 3x3 Matrix - */ - -#include "Matrix3.hpp" - -namespace math -{ -} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp deleted file mode 100644 index c7eb67ba7..000000000 --- a/src/lib/mathlib/math/Matrix3.hpp +++ /dev/null @@ -1,356 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix3.hpp - * - * 3x3 Matrix - */ - -#ifndef MATRIX3_HPP -#define MATRIX3_HPP - -#include "Vector3.hpp" -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - /** - * matrix data[row][col] - */ - T data[3][3]; - - /** - * struct for using arm_math functions - */ - arm_matrix_instance_f32 arm_mat; - - /** - * trivial ctor - * note that this ctor will not initialize elements - */ - Matrix3() { - arm_mat = {3, 3, &data[0][0]}; - } - - /** - * setting ctor - */ - Matrix3(const T d[3][3]) { - arm_mat = {3, 3, &data[0][0]}; - memcpy(data, d, sizeof(data)); - } - - /** - * setting ctor - */ - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = ax; - data[0][1] = ay; - data[0][2] = az; - data[1][0] = bx; - data[1][1] = by; - data[1][2] = bz; - data[2][0] = cx; - data[2][1] = cy; - data[2][2] = cz; - } - - /** - * casting from a vector3f to a matrix is the tilde operator - */ - Matrix3(const Vector3 &v) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = 0; - data[0][1] = -v.z; - data[0][2] = v.y; - data[1][0] = v.z; - data[1][1] = 0; - data[1][2] = -v.x; - data[2][0] = -v.y; - data[2][1] = v.x; - data[2][2] = 0; - } - - /** - * access by index - */ - inline T &operator ()(unsigned int row, unsigned int col) { - return data[row][col]; - } - - /** - * access to elements by index - */ - inline const T &operator ()(unsigned int row, unsigned int col) const { - return data[row][col]; - } - - /** - * set to value - */ - const Matrix3 &operator =(const Matrix3 &m) { - memcpy(data, m.data, sizeof(data)); - return *this; - } - - /** - * test for equality - */ - bool operator ==(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return false; - return true; - } - - /** - * test for inequality - */ - bool operator !=(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return true; - return false; - } - - /** - * negation - */ - Matrix3 operator -(void) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = -data[i][j]; - return res; - } - - /** - * addition - */ - Matrix3 operator +(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] + m(i, j); - return res; - } - - Matrix3 &operator +=(const Matrix3 &m) { - return *this = *this + m; - } - - /** - * subtraction - */ - Matrix3 operator -(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] - m(i, j); - return res; - } - - Matrix3 &operator -=(const Matrix3 &m) { - return *this = *this - m; - } - - /** - * uniform scaling - */ - Matrix3 operator *(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] * num; - return res; - } - - Matrix3 &operator *=(const T num) { - return *this = *this * num; - } - - Matrix3 operator /(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] / num; - return res; - } - - Matrix3 &operator /=(const T num) { - return *this = *this / num; - } - - /** - * multiplication by a vector - */ - Vector3 operator *(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, - data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, - data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); - } - - /** - * multiplication of transpose by a vector - */ - Vector3 mul_transpose(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, - data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, - data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); - } - - /** - * multiplication by another matrix - */ - Matrix3 operator *(const Matrix3 &m) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) - Matrix3 res; - arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), - data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), - data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), - data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), - data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), - data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), - data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), - data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), - data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); -#endif - } - - Matrix3 &operator *=(const Matrix3 &m) { - return *this = *this * m; - } - - /** - * transpose the matrix - */ - Matrix3 transposed(void) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float - Matrix3 res; - arm_mat_trans_f32(&arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0], data[1][0], data[2][0], - data[0][1], data[1][1], data[2][1], - data[0][2], data[1][2], data[2][2]); -#endif - } - - /** - * inverse the matrix - */ - Matrix3 inversed(void) const { - Matrix3 res; - arm_mat_inverse_f32(&arm_mat, &res.arm_mat); - return res; - } - - /** - * zero the matrix - */ - void zero(void) { - memset(data, 0, sizeof(data)); - } - - /** - * setup the identity matrix - */ - void identity(void) { - memset(data, 0, sizeof(data)); - data[0][0] = 1; - data[1][1] = 1; - data[2][2] = 1; - } - - /** - * check if any elements are NAN - */ - bool is_nan(void) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (isnan(data[i][j])) - return true; - return false; - } - - /** - * create a rotation matrix from given euler angles - * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf - */ - void from_euler(T roll, T pitch, T yaw) { - T cp = (T)cosf(pitch); - T sp = (T)sinf(pitch); - T sr = (T)sinf(roll); - T cr = (T)cosf(roll); - T sy = (T)sinf(yaw); - T cy = (T)cosf(yaw); - - data[0][0] = cp * cy; - data[0][1] = (sr * sp * cy) - (cr * sy); - data[0][2] = (cr * sp * cy) + (sr * sy); - data[1][0] = cp * sy; - data[1][1] = (sr * sp * sy) + (cr * cy); - data[1][2] = (cr * sp * sy) - (sr * cy); - data[2][0] = -sp; - data[2][1] = sr * cp; - data[2][2] = cr * cp; - } - - // create eulers from a rotation matrix - //void to_euler(float *roll, float *pitch, float *yaw); - - // apply an additional rotation from a body frame gyro vector - // to a rotation matrix. - //void rotate(const Vector3 &g); -}; - -typedef Matrix3 Matrix3f; -} - -#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 64b980ad8..1b0cb3c41 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,15 +44,17 @@ #include #include "../CMSIS/Include/arm_math.h" +#include "Vector.hpp" +#include "Matrix.hpp" namespace math { -class Quaternion { -public: - float real; - Vector3 imag; +template +class Matrix; +class Quaternion : public Vector<4> { +public: /** * trivial ctor */ @@ -62,7 +64,74 @@ public: /** * setting ctor */ - Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { + } + + Quaternion(const Vector<4> &v) : Vector(v) { + } + + Quaternion(const float *v) : Vector(v) { + } + + /** + * access to elements by index + */ + /* + inline float &operator ()(unsigned int i) { + return *(&a + i); + } + */ + + /** + * access to elements by index + */ + /* + inline const float &operator ()(unsigned int i) const { + return *(&a + i); + } + */ + + /** + * addition + */ + /* + const Quaternion operator +(const Quaternion &q) const { + return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); + } + */ + + /** + * subtraction + */ + /* + const Quaternion operator -(const Quaternion &q) const { + return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); + } + */ + + Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4,4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } + + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index cd00058b5..2df87c74b 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -42,38 +42,50 @@ #ifndef VECTOR_HPP #define VECTOR_HPP +#include #include #include "../CMSIS/Include/arm_math.h" namespace math { +template +class Vector; + template class VectorBase { public: + /** + * vector data + */ float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ arm_matrix_instance_f32 arm_col; /** * trivial ctor */ - VectorBase() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** - * setting ctor + * copy ctor */ -// VectorBase(const float *d) { - // memcpy(data, d, sizeof(data)); - //arm_col = {N, 1, &data[0]}; - //} + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } /** * setting ctor */ - VectorBase(const float d[]) : data(d) { + VectorBase(const float *d) { arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); } /** @@ -90,10 +102,18 @@ public: return data[i]; } + unsigned int getRows() { + return N; + } + + unsigned int getCols() { + return 1; + } + /** * test for equality */ - bool operator ==(const VectorBase &v) { + bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -103,7 +123,7 @@ public: /** * test for inequality */ - bool operator !=(const VectorBase &v) { + bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -113,98 +133,98 @@ public: /** * set to value */ - const VectorBase &operator =(const VectorBase &v) { + const Vector &operator =(const Vector &v) { memcpy(data, v.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - const VectorBase operator -(void) const { - VectorBase res; + const Vector operator -(void) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = -data[i]; + res.data[i] = -data[i]; return res; } /** * addition */ - const VectorBase operator +(const VectorBase &v) const { - VectorBase res; + const Vector operator +(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] + v(i); + res.data[i] = data[i] + v(i); return res; } /** * subtraction */ - const VectorBase operator -(const VectorBase &v) const { - VectorBase res; + const Vector operator -(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] - v(i); + res.data[i] = data[i] - v(i); return res; } /** * uniform scaling */ - const VectorBase operator *(const float num) const { - VectorBase temp(*this); + const Vector operator *(const float num) const { + Vector temp(*this); return temp *= num; } /** * uniform scaling */ - const VectorBase operator /(const float num) const { - VectorBase temp(*this); + const Vector operator /(const float num) const { + Vector temp(*reinterpret_cast*>(this)); return temp /= num; } /** * addition */ - const VectorBase &operator +=(const VectorBase &v) { + const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *this; + return *reinterpret_cast*>(this); } /** * subtraction */ - const VectorBase &operator -=(const VectorBase &v) { + const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator *=(const float num) { + const Vector &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator /=(const float num) { + const Vector &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *this; + return *reinterpret_cast*>(this); } /** * dot product */ - float operator *(const VectorBase &v) const { - float res; + float operator *(const Vector &v) const { + float res = 0.0f; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); return res; @@ -221,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *this); + return sqrtf(*this * *reinterpret_cast*>(this)); } /** @@ -234,46 +254,174 @@ public: /** * returns the normalized version of this vector */ - VectorBase normalized() const { + Vector normalized() const { return *this / length(); } + + void print(void) { + printf("[ "); + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + printf("]\n"); + } }; template class Vector : public VectorBase { public: + using VectorBase::operator *; + + Vector() : VectorBase() { + } + + Vector(const float d[]) : VectorBase(d) { + } + + Vector(const Vector &v) : VectorBase(v) { + } + + Vector(const VectorBase &v) : VectorBase(v) { + } + /** * set to value */ const Vector &operator =(const Vector &v) { + this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } }; +template <> +class Vector<2> : public VectorBase<2> { +public: + Vector() : VectorBase<2>() { + } + + Vector(const float x, const float y) : VectorBase() { + data[0] = x; + data[1] = y; + } + + Vector(const Vector<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const VectorBase<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float cross(const Vector<2> &b) const { + return data[0]*b.data[1] - data[1]*b.data[0]; + } + + float operator %(const Vector<2> &v) const { + return cross(v); + } + +}; + template <> class Vector<3> : public VectorBase<3> { public: - Vector<3>() { + Vector() { arm_col = {3, 1, &this->data[0]}; } - Vector<3>(const float x, const float y, const float z) { + Vector(const float x, const float y, const float z) { + arm_col = {3, 1, &this->data[0]}; data[0] = x; data[1] = y; data[2] = z; + } + + Vector(const Vector<3> &v) : VectorBase<3>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + } + + /** + * setting ctor + */ + Vector(const float d[]) { arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + +template <> +class Vector<4> : public VectorBase<4> { +public: + Vector() : VectorBase() {} + + Vector(const float x, const float y, const float z, const float t) : VectorBase() { + data[0] = x; + data[1] = y; + data[2] = z; + data[3] = t; } + Vector(const Vector<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + Vector(const VectorBase<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + /** + * setting ctor + */ + /* + Vector(const float d[]) { + arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } +*/ /** * set to value */ + /* const Vector<3> &operator =(const Vector<3> &v) { data[0] = v.data[0]; data[1] = v.data[1]; data[2] = v.data[2]; return *this; } + */ }; } diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp deleted file mode 100644 index 501740614..000000000 --- a/src/lib/mathlib/math/Vector2.hpp +++ /dev/null @@ -1,269 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2.hpp - * - * 2D Vector - */ - -#ifndef VECTOR2_HPP -#define VECTOR2_HPP - -#include - -namespace math -{ - -template -class Vector2 { -public: - T x, y; - - /** - * trivial ctor - */ - Vector2() { - } - - /** - * setting ctor - */ - Vector2(const T x0, const T y0): x(x0), y(y0) { - } - - /** - * setter - */ - void set(const T x0, const T y0) { - x = x0; - y = y0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector2 &v) { - return (x == v.x && y == v.y); - } - - /** - * test for inequality - */ - bool operator !=(const Vector2 &v) { - return (x != v.x || y != v.y); - } - - /** - * set to value - */ - const Vector2 &operator =(const Vector2 &v) { - x = v.x; - y = v.y; - return *this; - } - - /** - * negation - */ - const Vector2 operator -(void) const { - return Vector2(-x, -y); - } - - /** - * addition - */ - const Vector2 operator +(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); - } - - /** - * subtraction - */ - const Vector2 operator -(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); - } - - /** - * uniform scaling - */ - const Vector2 operator *(const T num) const { - Vector2 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector2 operator /(const T num) const { - Vector2 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector2 &operator +=(const Vector2 &v) { - x += v.x; - y += v.y; - return *this; - } - - /** - * subtraction - */ - const Vector2 &operator -=(const Vector2 &v) { - x -= v.x; - y -= v.y; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator *=(const T num) { - x *= num; - y *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator /=(const T num) { - x /= num; - y /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector2 &v) const { - return x * v.x + y * v.y; - } - - /** - * cross product - */ - const float operator %(const Vector2 &v) const { - return x * v.y - y * v.x; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector2 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector2 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector2 projected(const Vector2 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector2 &v1, const Vector2 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector2 Vector2f; -} - -#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index a0c3b9290..000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.hpp - * - * 3D Vector - */ - -#ifndef VECTOR3_HPP -#define VECTOR3_HPP - -#include -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -template -class Vector3 { -public: - T x, y, z; - arm_matrix_instance_f32 arm_col; - - /** - * trivial ctor - */ - Vector3() { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { - arm_col = {3, 1, &x}; - } - - /** - * setter - */ - void set(const T x0, const T y0, const T z0) { - x = x0; - y = y0; - z = z0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector3 &v) { - return (x == v.x && y == v.y && z == v.z); - } - - /** - * test for inequality - */ - bool operator !=(const Vector3 &v) { - return (x != v.x || y != v.y || z != v.z); - } - - /** - * set to value - */ - const Vector3 &operator =(const Vector3 &v) { - x = v.x; - y = v.y; - z = v.z; - return *this; - } - - /** - * negation - */ - const Vector3 operator -(void) const { - return Vector3(-x, -y, -z); - } - - /** - * addition - */ - const Vector3 operator +(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); - } - - /** - * subtraction - */ - const Vector3 operator -(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); - } - - /** - * uniform scaling - */ - const Vector3 operator *(const T num) const { - Vector3 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector3 operator /(const T num) const { - Vector3 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector3 &operator +=(const Vector3 &v) { - x += v.x; - y += v.y; - z += v.z; - return *this; - } - - /** - * subtraction - */ - const Vector3 &operator -=(const Vector3 &v) { - x -= v.x; - y -= v.y; - z -= v.z; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator *=(const T num) { - x *= num; - y *= num; - z *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator /=(const T num) { - x /= num; - y /= num; - z /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector3 &v) const { - return x * v.x + y * v.y + z * v.z; - } - - /** - * cross product - */ - const Vector3 operator %(const Vector3 &v) const { - Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - return temp; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector3 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector3 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector3 projected(const Vector3 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector3 &v1, const Vector3 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector3 Vector3f; -} - -#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index d8e95b46b..9e03855c5 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -42,10 +42,7 @@ #pragma once #include "math/Vector.hpp" -#include "math/Vector2.hpp" -#include "math/Vector3.hpp" #include "math/Matrix.hpp" -#include "math/Matrix3.hpp" #include "math/Quaternion.hpp" #include "math/Limits.hpp" diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 50951c617..191e2da73 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,7 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Matrix3.cpp \ math/Limits.cpp # diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7..6533eb7cf 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), + F(), + G(), + P(), + P0(), + V(), // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), + HAtt(), + RAtt(), // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), + HPos(), + RPos(), // attitude representations C_nb(), q(), @@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb.from_quaternion(q); // HPos is constant HPos(0, 3) = 1.0f; @@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb.from_quaternion(q); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +578,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +594,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +613,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,9 +630,9 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -669,7 +667,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +676,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +686,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,9 +696,9 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -735,7 +733,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a6..46ee4b6c8 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a..36b75dd58 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix board_rotation(3, 3); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix board_rotation_t = board_rotation.transpose(); - math::Vector3 accel_offs_vec; - accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix accel_T_mat(3, 3); - accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..653d4b6b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb; + C_nb.from_quaternion(q); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444a..04307d96b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -422,11 +422,11 @@ Navigator::task_main() mission_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); // Global position - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* AUTONOMOUS FLIGHT */ @@ -436,19 +436,19 @@ Navigator::task_main() if (_mission_valid) { // Next waypoint - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); + prev_wp(0) = _global_triplet.previous.lat / 1e7f; + prev_wp(1) = _global_triplet.previous.lon / 1e7f; } else { /* * No valid next waypoint, go for heading hold. * This is automatically handled by the L1 library. */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + prev_wp(0) = _global_triplet.current.lat / 1e7f; + prev_wp(1) = _global_triplet.current.lon / 1e7f; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec..5baab4a5d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -211,8 +211,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ struct { @@ -457,8 +457,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), + _board_rotation(), + _external_mag_rotation(), _mag_is_external(false) { @@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 305ebbefa..d2e1a93e3 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -75,15 +75,6 @@ int test_mathlib(int argc, char *argv[]) Matrix<3,3> mres3; Matrix<4,4> mres4; - Matrix3f m3old; - m3old.identity(); - Vector3f v3old; - v3old.x = 1.0f; - v3old.y = 2.0f; - v3old.z = 3.0f; - Vector3f vres3old; - Matrix3f mres3old; - unsigned int n = 60000; hrt_abstime t0, t1; @@ -95,13 +86,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3old = m3old * v3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres3 = m3 * m3; @@ -109,13 +93,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3old = m3old * m3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres4 = m4 * m4; -- cgit v1.2.3 From 411428b8e448f773d104704449b08261197a4b27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 11:45:03 +0100 Subject: remove dataman from tests (as in upstream master) --- src/systemcmds/tests/tests_main.c | 1 - 1 file changed, 1 deletion(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 30ae5b4ec..1088a4407 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -107,7 +107,6 @@ const struct { {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, - {"dataman", test_dataman, OPT_NOALLTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From 9dfe366e908ce0100875996c3ea0d4cfdfcc24bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 24 Dec 2013 23:56:28 +0400 Subject: mathlib: Vector class major cleanup --- src/lib/mathlib/math/Matrix.hpp | 27 +--- src/lib/mathlib/math/Quaternion.hpp | 48 +++++- src/lib/mathlib/math/Vector.hpp | 195 +++++++++++------------- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 8 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sensors/sensors.cpp | 6 +- src/systemcmds/tests/test_mathlib.cpp | 114 ++++++++------ 7 files changed, 209 insertions(+), 192 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 7ed8879a7..9896a16d0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -94,11 +94,11 @@ public: return data[row][col]; } - unsigned int getRows() { + unsigned int get_rows() { return M; } - unsigned int getCols() { + unsigned int get_cols() { return N; } @@ -295,11 +295,7 @@ public: return res; } }; -} - -#include "Quaternion.hpp" -namespace math { template <> class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: @@ -329,25 +325,6 @@ public: return res; } - /** - * create a rotation matrix from given quaternion - */ - void from_quaternion(const Quaternion &q) { - float aSq = q.data[0] * q.data[0]; - float bSq = q.data[1] * q.data[1]; - float cSq = q.data[2] * q.data[2]; - float dSq = q.data[3] * q.data[3]; - data[0][0] = aSq + bSq - cSq - dSq; - data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); - data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); - data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); - data[1][1] = aSq - bSq + cSq - dSq; - data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); - data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); - data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); - data[2][2] = aSq - bSq - cSq + dSq; - } - /** * create a rotation matrix from given euler angles * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 3735fb3d3..c19dbd29c 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -58,7 +58,7 @@ public: /** * trivial ctor */ - Quaternion() { + Quaternion() : Vector() { } /** @@ -70,10 +70,29 @@ public: Quaternion(const Vector<4> &v) : Vector(v) { } - Quaternion(const float *v) : Vector(v) { + Quaternion(const Quaternion &q) : Vector(q) { } - Quaternion derivative(const Vector<3> &w) { + Quaternion(const float v[4]) : Vector(v) { + } + + using Vector<4>::operator *; + + /** + * multiplication + */ + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } + + /** + * derivative + */ + const Quaternion derivative(const Vector<3> &w) { float dataQ[] = { data[0], -data[1], -data[2], -data[3], data[1], data[0], -data[3], data[2], @@ -85,6 +104,9 @@ public: return Q * v * 0.5f; } + /** + * set quaternion to rotation defined by euler angles + */ void from_euler(float roll, float pitch, float yaw) { double cosPhi_2 = cos(double(roll) / 2.0); double sinPhi_2 = sin(double(roll) / 2.0); @@ -97,6 +119,26 @@ public: data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 744402e21..d579ecf73 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,13 +37,12 @@ /** * @file Vector.hpp * - * Generic Vector + * Vector class */ #ifndef VECTOR_HPP #define VECTOR_HPP -#include #include #include #include "../CMSIS/Include/arm_math.h" @@ -84,7 +84,7 @@ public: /** * setting ctor */ - VectorBase(const float *d) { + VectorBase(const float d[N]) { arm_col = {N, 1, &data[0]}; memcpy(data, d, sizeof(data)); } @@ -92,31 +92,30 @@ public: /** * access to elements by index */ - inline float &operator ()(unsigned int i) { + float &operator ()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - inline const float &operator ()(unsigned int i) const { + float operator ()(const unsigned int i) const { return data[i]; } - unsigned int getRows() { + /** + * get rows number + */ + unsigned int get_size() const { return N; } - unsigned int getCols() { - return 1; - } - /** * test for equality */ bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return false; return true; } @@ -126,7 +125,7 @@ public: */ bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return true; return false; } @@ -155,7 +154,7 @@ public: const Vector operator +(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v(i); + res.data[i] = data[i] + v.data[i]; return res; } @@ -165,7 +164,7 @@ public: const Vector operator -(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v(i); + res.data[i] = data[i] - v.data[i]; return res; } @@ -173,16 +172,20 @@ public: * uniform scaling */ const Vector operator *(const float num) const { - Vector temp(*this); - return temp *= num; + Vector res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + return res; } /** * uniform scaling */ const Vector operator /(const float num) const { - Vector temp(*static_cast*>(this)); - return temp /= num; + Vector res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } /** @@ -190,7 +193,7 @@ public: */ const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] += v(i); + data[i] += v.data[i]; return *static_cast*>(this); } @@ -199,7 +202,7 @@ public: */ const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] -= v(i); + data[i] -= v.data[i]; return *static_cast*>(this); } @@ -227,7 +230,7 @@ public: float operator *(const Vector &v) const { float res = 0.0f; for (unsigned int i = 0; i < N; i++) - res += data[i] * v(i); + res += data[i] * v.data[i]; return res; } @@ -235,14 +238,20 @@ public: * gets the length of this vector squared */ float length_squared() const { - return (*this * *this); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return res; } /** * gets the length of this vector */ float length() const { - return sqrtf(*this * *static_cast*>(this)); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return sqrtf(res); } /** @@ -277,25 +286,17 @@ public: template class __EXPORT Vector : public VectorBase { public: - using VectorBase::operator *; + //using VectorBase::operator *; + Vector() : VectorBase() {} - Vector() : VectorBase() { - } + Vector(const Vector &v) : VectorBase(v) {} - Vector(const float d[]) : VectorBase(d) { - } - - Vector(const Vector &v) : VectorBase(v) { - } - - Vector(const VectorBase &v) : VectorBase(v) { - } + Vector(const float d[N]) : VectorBase(d) {} /** * set to value */ const Vector &operator =(const Vector &v) { - this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } @@ -304,22 +305,22 @@ public: template <> class __EXPORT Vector<2> : public VectorBase<2> { public: - Vector() : VectorBase<2>() { - } + Vector() : VectorBase<2>() {} - Vector(const float x, const float y) : VectorBase() { - data[0] = x; - data[1] = y; - } - - Vector(const Vector<2> &v) : VectorBase() { + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<2>() { data[0] = v.data[0]; data[1] = v.data[1]; } - Vector(const VectorBase<2> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { + data[0] = x; + data[1] = y; } /** @@ -331,55 +332,49 @@ public: return *this; } - float cross(const Vector<2> &b) const { - return data[0]*b.data[1] - data[1]*b.data[0]; - } - float operator %(const Vector<2> &v) const { - return cross(v); + return data[0] * v.data[1] - data[1] * v.data[0]; } - }; template <> class __EXPORT Vector<3> : public VectorBase<3> { public: - Vector() { - arm_col = {3, 1, &this->data[0]}; - } + Vector() : VectorBase<3>() {} - Vector(const float x, const float y, const float z) { - arm_col = {3, 1, &this->data[0]}; - data[0] = x; - data[1] = y; - data[2] = z; + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; } - Vector(const Vector<3> &v) : VectorBase<3>() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; } - /** - * setting ctor - */ - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; } /** * set to value */ const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; return *this; } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } }; template <> @@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const float x, const float y, const float z, const float t) : VectorBase() { - data[0] = x; - data[1] = y; - data[2] = z; - data[3] = t; + Vector(const Vector &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; } - Vector(const Vector<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; } - Vector(const VectorBase<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; } - /** - * setting ctor - */ - /* - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; - } -*/ /** * set to value */ - /* - const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; return *this; } - */ }; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 9d3ef07f2..aca3fe7b6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt) } // C_nb update - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // euler update Vector<3> euler = C_nb.to_euler(); @@ -628,7 +628,7 @@ int KalmanNav::correctAtt() Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { @@ -694,7 +694,7 @@ int KalmanNav::correctPos() Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 653d4b6b3..b4f7f2dfe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb; - C_nb.from_quaternion(q); + math::Matrix<3,3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eea9438f7..9e2eeafa4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -465,8 +465,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(), - _external_mag_rotation(), _mag_is_external(false) { @@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index d2e1a93e3..e654e0f81 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -48,6 +48,8 @@ #include "tests.h" +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + using namespace math; const char* formatResult(bool res) { @@ -58,60 +60,82 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix<3,3> m3; - m3.identity(); - Matrix<4,4> m4; - m4.identity(); - Vector<3> v3; - v3(0) = 1.0f; - v3(1) = 2.0f; - v3(2) = 3.0f; - Vector<4> v4; - v4(0) = 1.0f; - v4(1) = 2.0f; - v4(2) = 3.0f; - v4(3) = 4.0f; - Vector<3> vres3; - Matrix<3,3> mres3; - Matrix<4,4> mres4; - - unsigned int n = 60000; + Vector<2> v2(1.0f, 2.0f); + Vector<3> v3(1.0f, 2.0f, 3.0f); + Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); + Vector<10> v10; + v10.zero(); - hrt_abstime t0, t1; + float data2[2] = {1.0f, 2.0f}; + float data3[3] = {1.0f, 2.0f, 3.0f}; + float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + float data10[10]; - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3 = m3 * v3; + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + TEST_OP("Constructor Vector<2>()", Vector<2> v); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3 * m3; + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + TEST_OP("Constructor Vector<3>()", Vector<3> v); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres4 = m4 * m4; + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + TEST_OP("Constructor Vector<4>()", Vector<4> v); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); } - t1 = hrt_absolute_time(); - warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.transposed(); + { + TEST_OP("Constructor Vector<10>()", Vector<10> v); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); } - t1 = hrt_absolute_time(); - warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.inversed(); - } - t1 = hrt_absolute_time(); - warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } -- cgit v1.2.3 From 8ed193d1159dd64e3bd44668e75aac8b71fa3fa2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Dec 2013 13:03:36 +0400 Subject: mathlib: Matrix and Quaternion cleanup and bugfixes. Copyright updated. --- src/lib/mathlib/math/Matrix.hpp | 57 +++++++++++------- src/lib/mathlib/math/Quaternion.hpp | 40 +++++++------ src/lib/mathlib/math/Vector.hpp | 20 +++---- src/modules/att_pos_estimator_ekf/kalman_main.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 70 ++++++++++++++--------- 5 files changed, 114 insertions(+), 75 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9896a16d0..584e5e81b 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +37,7 @@ /** * @file Matrix.hpp * - * Generic Matrix + * Matrix class */ #ifndef MATRIX_HPP @@ -51,8 +52,6 @@ namespace math template class __EXPORT Matrix; -class __EXPORT Quaternion; - // MxN matrix with float elements template class __EXPORT MatrixBase { @@ -75,6 +74,14 @@ public: arm_mat = {M, N, &data[0][0]}; } + /** + * copyt ctor + */ + MatrixBase(const MatrixBase &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + MatrixBase(const float *d) { arm_mat = {M, N, &data[0][0]}; memcpy(data, d, sizeof(data)); @@ -83,29 +90,35 @@ public: /** * access by index */ - inline float &operator ()(unsigned int row, unsigned int col) { + float &operator ()(const unsigned int row, const unsigned int col) { return data[row][col]; } /** * access by index */ - inline const float &operator ()(unsigned int row, unsigned int col) const { + float operator ()(const unsigned int row, const unsigned int col) const { return data[row][col]; } - unsigned int get_rows() { + /** + * get rows number + */ + unsigned int get_rows() const { return M; } - unsigned int get_cols() { + /** + * get columns number + */ + unsigned int get_cols() const { return N; } /** * test for equality */ - bool operator ==(const Matrix &m) { + bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) if (data[i][j] != m.data[i][j]) @@ -116,7 +129,7 @@ public: /** * test for inequality */ - bool operator !=(const Matrix &m) { + bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) if (data[i][j] != m.data[i][j]) @@ -209,7 +222,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] /= num; - return *this; + return *static_cast*>(this); } /** @@ -272,11 +285,11 @@ class __EXPORT Matrix : public MatrixBase { public: using MatrixBase::operator *; - Matrix() : MatrixBase() { - } + Matrix() : MatrixBase() {} - Matrix(const float *d) : MatrixBase(d) { - } + Matrix(const Matrix &m) : MatrixBase(m) {} + + Matrix(const float *d) : MatrixBase(d) {} /** * set to value @@ -301,11 +314,11 @@ class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; - Matrix() : MatrixBase<3, 3>() { - } + Matrix() : MatrixBase<3, 3>() {} - Matrix(const float *d) : MatrixBase<3, 3>(d) { - } + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} /** * set to value @@ -348,6 +361,9 @@ public: data[2][2] = cr * cp; } + /** + * get euler angles from rotation matrix + */ Vector<3> to_euler(void) const { Vector<3> euler; euler.data[1] = asinf(-data[2][0]); @@ -364,6 +380,7 @@ public: euler.data[0] = atan2f(data[2][1], data[2][2]); euler.data[2] = atan2f(data[1][0], data[0][0]); } + return euler; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index c19dbd29c..54d4e72ab 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +37,7 @@ /** * @file Quaternion.hpp * - * Quaternion + * Quaternion class */ #ifndef QUATERNION_HPP @@ -50,31 +51,32 @@ namespace math { -template -class Matrix; - -class Quaternion : public Vector<4> { +class __EXPORT Quaternion : public Vector<4> { public: /** * trivial ctor */ - Quaternion() : Vector() { - } + Quaternion() : Vector<4>() {} /** - * setting ctor + * copy ctor */ - Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { - } + Quaternion(const Quaternion &q) : Vector<4>(q) {} - Quaternion(const Vector<4> &v) : Vector(v) { - } + /** + * casting from vector + */ + Quaternion(const Vector<4> &v) : Vector<4>(v) {} - Quaternion(const Quaternion &q) : Vector(q) { - } + /** + * setting ctor + */ + Quaternion(const float d[4]) : Vector<4>(d) {} - Quaternion(const float v[4]) : Vector(v) { - } + /** + * setting ctor + */ + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} using Vector<4>::operator *; @@ -138,8 +140,10 @@ public: R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); R.data[2][2] = aSq - bSq - cSq + dSq; + return R; } }; + } #endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index d579ecf73..6bfcc96b6 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -68,6 +68,7 @@ public: /** * trivial ctor + * note that this ctor will not initialize elements */ VectorBase() { arm_col = {N, 1, &data[0]}; @@ -104,7 +105,7 @@ public: } /** - * get rows number + * get vector size */ unsigned int get_size() const { return N; @@ -113,7 +114,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return false; @@ -123,7 +124,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return true; @@ -286,10 +287,9 @@ public: template class __EXPORT Vector : public VectorBase { public: - //using VectorBase::operator *; Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase(v) {} + Vector(const Vector &v) : VectorBase(v) {} Vector(const float d[N]) : VectorBase(d) {} @@ -307,8 +307,8 @@ class __EXPORT Vector<2> : public VectorBase<2> { public: Vector() : VectorBase<2>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : VectorBase<2>() { + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<2> &v) : VectorBase<2>() { data[0] = v.data[0]; data[1] = v.data[1]; } @@ -342,8 +342,8 @@ class __EXPORT Vector<3> : public VectorBase<3> { public: Vector() : VectorBase<3>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : VectorBase<3>() { + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<3> &v) : VectorBase<3>() { for (unsigned int i = 0; i < 3; i++) data[i] = v.data[i]; } @@ -382,7 +382,7 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase<4>() { + Vector(const Vector<4> &v) : VectorBase<4>() { for (unsigned int i = 0; i < 4; i++) data[i] = v.data[i]; } diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 70ba628f3..3d20d4d2d 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 8096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index e654e0f81..693a208ba 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -60,25 +60,15 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Vector<2> v2(1.0f, 2.0f); - Vector<3> v3(1.0f, 2.0f, 3.0f); - Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); - Vector<10> v10; - v10.zero(); - - float data2[2] = {1.0f, 2.0f}; - float data3[3] = {1.0f, 2.0f, 3.0f}; - float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; - float data10[10]; - { Vector<2> v; Vector<2> v1(1.0f, 2.0f); Vector<2> v2(1.0f, -1.0f); - TEST_OP("Constructor Vector<2>()", Vector<2> v); - TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); - TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); - TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f)); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector<2>()", Vector<2> v3); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); TEST_OP("Vector<2> = Vector<2>", v = v1); TEST_OP("Vector<2> + Vector<2>", v + v1); TEST_OP("Vector<2> - Vector<2>", v - v1); @@ -92,10 +82,11 @@ int test_mathlib(int argc, char *argv[]) Vector<3> v; Vector<3> v1(1.0f, 2.0f, 0.0f); Vector<3> v2(1.0f, -1.0f, 2.0f); - TEST_OP("Constructor Vector<3>()", Vector<3> v); - TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); - TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); - TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f)); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector<3>()", Vector<3> v3); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); TEST_OP("Vector<3> = Vector<3>", v = v1); TEST_OP("Vector<3> + Vector<3>", v + v1); TEST_OP("Vector<3> - Vector<3>", v - v1); @@ -119,10 +110,11 @@ int test_mathlib(int argc, char *argv[]) Vector<4> v; Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); - TEST_OP("Constructor Vector<4>()", Vector<4> v); - TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); - TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); - TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f)); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Vector<4> v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); TEST_OP("Vector<4> = Vector<4>", v = v1); TEST_OP("Vector<4> + Vector<4>", v + v1); TEST_OP("Vector<4> - Vector<4>", v - v1); @@ -132,9 +124,35 @@ int test_mathlib(int argc, char *argv[]) } { - TEST_OP("Constructor Vector<10>()", Vector<10> v); - TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); - TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); + Vector<10> v1; + v1.zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Vector<10> v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> m2; + m2.identity(); + Vector<3> v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); + TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); + TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); + } + + { + Matrix<10, 10> m1; + m1.identity(); + Matrix<10, 10> m2; + m2.identity(); + Vector<10> v1; + v1.zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); } return 0; -- cgit v1.2.3 From b92b08ae62b2da027e9fcd15a3a45a9125ea8355 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 2 Feb 2014 00:42:15 -0500 Subject: Optimize and update data manager docs Move repeated code to common function Update missing and misleading comments Fix data manager test compile errors No functional changes --- src/modules/dataman/dataman.c | 178 ++++++++++++++++++++++-------------- src/systemcmds/tests/test_dataman.c | 8 +- 2 files changed, 112 insertions(+), 74 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dc2d6c312..fa88dfaff 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman"; /* The data manager work queues */ typedef struct { - sq_queue_t q; - sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; - unsigned max_size; + sq_queue_t q; /* Nuttx queue */ + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; /* Current size of queue */ + unsigned max_size; /* Maximum queue size reached */ } work_q_t; -static work_q_t g_free_q; -static work_q_t g_work_q; +static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ +static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ -sem_t g_work_queued_sema; +sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -#define DM_SECTOR_HDR_SIZE 4 -static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; +#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); - sem_init(&(q->mutex), 1, 1); - q->size = q->max_size = 0; + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + q->size = q->max_size = 0; /* Queue is initially empty */ } -static void destroy_q(work_q_t *q) +static inline void +destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void lock_queue(work_q_t *q) { - sem_wait(&(q->mutex)); + sem_wait(&(q->mutex)); /* Acquire the queue lock */ } static inline void unlock_queue(work_q_t *q) { - sem_post(&(q->mutex)); + sem_post(&(q->mutex)); /* Release the queue lock */ } static work_q_item_t * @@ -172,54 +173,47 @@ create_work_item(void) { work_q_item_t *item; + /* Try to reuse item from free item queue */ lock_queue(&g_free_q); if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) g_free_q.size--; unlock_queue(&g_free_q); + /* If we there weren't any free items then obtain memory for a new one */ if (item == NULL) item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we got one then lock the item*/ if (item) sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + /* return the item pointer, or NULL if all failed */ return item; } /* Work queue management functions */ -static void -enqueue_work_item(work_q_item_t *item) -{ - /* put the work item on the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - - if (++g_work_q.size > g_work_q.max_size) - g_work_q.max_size = g_work_q.size; - - unlock_queue(&g_work_q); - /* tell the work thread that work is available */ - sem_post(&g_work_queued_sema); -} - -static void +static inline void destroy_work_item(work_q_item_t *item) { - sem_destroy(&item->wait_sem); + sem_destroy(&item->wait_sem); /* Destroy the item lock */ + /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); + /* Update the queue size and potentially the maximum queue size */ if (++g_free_q.size > g_free_q.max_size) g_free_q.max_size = g_free_q.size; unlock_queue(&g_free_q); } -static work_q_item_t * +static inline work_q_item_t * dequeue_work_item(void) { work_q_item_t *work; + + /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) @@ -229,6 +223,32 @@ dequeue_work_item(void) return work; } +static int +enqueue_work_item_and_wait_for_result(work_q_item_t *item) +{ + /* put the work item at the end of the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + /* Adjust the queue size and potentially the maximum queue size */ + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); + + /* wait for the result */ + sem_wait(&item->wait_sem); + + int result = item->result; + + destroy_work_item(item); + + return result; +} + /* Calculate the offset in file of specific item */ static int calculate_offset(dm_item_t item, unsigned char index) @@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index) * * byte 0: Length of user data item * byte 1: Persistence of this data item + * byte 2: Unused (for future use) + * byte 3: Unused (for future use) * byte DM_SECTOR_HDR_SIZE... : data item value * * The total size must not exceed k_sector_size @@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v len = -1; + /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); + fsync(g_task_fd); /* Make sure data is written to physical media */ + /* Make sure the write succeeded */ if (len != count) return -1; @@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) if (lseek(g_task_fd, offset, SEEK_SET) == offset) len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); - /* Check for length issues */ + /* Check for read error */ if (len < 0) return -1; + /* A zero length entry is a empty entry */ if (len == 0) buffer[0] = 0; + /* See if we got data */ if (buffer[0] > 0) { + /* We got more than requested!!! */ if (buffer[0] > count) return -1; @@ -340,11 +369,14 @@ _clear(dm_item_t item) { int i, result = 0; + /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); + /* Check for item type out of range */ if (offset < 0) return -1; + /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { char buf[1]; @@ -353,9 +385,11 @@ _clear(dm_item_t item) break; } + /* Avoid SD flash wear by only doing writes where necessary */ if (read(g_task_fd, buf, 1) < 1) break; + /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { if (lseek(g_task_fd, offset, SEEK_SET) != offset) { result = -1; @@ -373,6 +407,7 @@ _clear(dm_item_t item) offset += k_sector_size; } + /* Make sure data is actually written to physical media */ fsync(g_task_fd); return result; } @@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a write request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_write_func; work->write_params.item = item; @@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work->write_params.persistence = persistence; work->write_params.buf = buf; work->write_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } /* Retrieve from the data manager file */ @@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a read request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_read_func; work->read_params.item = item; work->read_params.index = index; work->read_params.buf = buf; work->read_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } __EXPORT int @@ -504,21 +535,19 @@ dm_clear(dm_item_t item) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a clear request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_clear_func; work->clear_params.item = item; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } /* Tell the data manager about the type of the last reset */ @@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a restart request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_restart_func; work->restart_params.reason = reason; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } static int @@ -570,24 +597,29 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); + /* We use two file descriptors, one for the caller context and one for the worker thread */ + /* They are actually the same but we need to some way to reject caller request while the */ + /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ @@ -595,7 +627,7 @@ task_main(int argc, char *argv[]) /* do we need to exit ??? */ if ((g_task_should_exit) && (g_fd >= 0)) { - /* Close the file handle to stop further queueing */ + /* Close the file handle to stop further queuing */ g_fd = -1; } @@ -607,6 +639,7 @@ task_main(int argc, char *argv[]) /* Empty the work queue */ while ((work = dequeue_work_item())) { + /* handle each work item with the appropriate handler */ switch (work->func) { case dm_write_func: g_func_counts[dm_write_func]++; @@ -647,7 +680,7 @@ task_main(int argc, char *argv[]) close(g_task_fd); g_task_fd = -1; - /* Empty the work queue */ + /* The work queue is now empty, empty the free queue */ for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; @@ -669,7 +702,7 @@ start(void) sem_init(&g_init_sema, 1, 0); - /* start the task */ + /* start the worker thread */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { warn("task start failed"); return -1; @@ -704,7 +737,7 @@ stop(void) static void usage(void) { - errx(1, "usage: dataman {start|stop|status}"); + errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); } int @@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[]) exit(0); } + /* Worker thread should be running for all other commands */ if (g_fd < 0) errx(1, "not running"); @@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[]) stop(); else if (!strcmp(argv[1], "status")) status(); + else if (!strcmp(argv[1], "poweronrestart")) + dm_restart(DM_INIT_REASON_POWER_ON); + else if (!strcmp(argv[1], "inflightrestart")) + dm_restart(DM_INIT_REASON_IN_FLIGHT); else usage(); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 5b121e34e..1f844a97d 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } -- cgit v1.2.3 From 20618a611b5cd719cab803823b2f3a08d9a498f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:44:05 -0800 Subject: Fixed a number of compile warnings in mount tests. --- src/systemcmds/tests/test_mount.c | 38 +++++++++++++++++--------------------- 1 file changed, 17 insertions(+), 21 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 44e34d9ef..4b6303cfb 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -141,8 +141,8 @@ test_mount(int argc, char *argv[]) /* announce mode switch */ if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); } @@ -162,7 +162,7 @@ test_mount(int argc, char *argv[]) } char buf[64]; - int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + (void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); lseek(cmd_fd, 0, SEEK_SET); write(cmd_fd, buf, strlen(buf) + 1); fsync(cmd_fd); @@ -174,8 +174,8 @@ test_mount(int argc, char *argv[]) printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); for (unsigned a = 0; a < alignments; a++) { @@ -185,22 +185,20 @@ test_mount(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - hrt_abstime start, end; int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); - if (wret != chunk_sizes[c]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) @@ -214,8 +212,8 @@ test_mount(int argc, char *argv[]) fsync(fd); } else { printf("#"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); } } @@ -224,12 +222,10 @@ test_mount(int argc, char *argv[]) } printf("."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(200000); - end = hrt_absolute_time(); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -237,7 +233,7 @@ test_mount(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret != (int)chunk_sizes[c]) { warnx("READ ERROR!"); return 1; } @@ -245,7 +241,7 @@ test_mount(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j + a]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); compare_ok = false; @@ -271,16 +267,16 @@ test_mount(int argc, char *argv[]) } } - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); /* we always reboot for the next test if we get here */ warnx("Iteration done, rebooting.."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); systemreset(false); -- cgit v1.2.3 From a7012a54180386b5dc7f86fe6a1564690ecc8240 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 08:34:55 +1100 Subject: tests: added "tests file2" filesystem test this is useful for testing for filesystem corruption on cluster boundaries --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_file2.c | 196 ++++++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 199 insertions(+) create mode 100644 src/systemcmds/tests/test_file2.c (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index acf28c35b..622a0faf3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -26,6 +26,7 @@ SRCS = test_adc.c \ test_mixer.cpp \ test_mathlib.cpp \ test_file.c \ + test_file2.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c new file mode 100644 index 000000000..ef555f6c3 --- /dev/null +++ b/src/systemcmds/tests/test_file2.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_file2.c + * + * File write test. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define FLAG_FSYNC 1 +#define FLAG_LSEEK 2 + +/* + return a predictable value for any file offset to allow detection of corruption + */ +static uint8_t get_value(uint32_t ofs) +{ + union { + uint32_t ofs; + uint8_t buf[4]; + } u; + u.ofs = ofs; + return u.buf[ofs % 4]; +} + +static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) +{ + printf("Testing on %s with write_chunk=%u write_size=%u\n", + filename, (unsigned)write_chunk, (unsigned)write_size); + + uint32_t ofs = 0; + int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { + perror(filename); + exit(1); + } + + // create a file of size write_size, in write_chunk blocks + uint8_t counter = 0; + while (ofs < write_size) { + uint8_t buffer[write_chunk]; + for (uint16_t j=0; j 0) { + filename = argv[0]; + } + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + test_corruption(filename, write_chunk, write_size, flags); + return 0; +} + diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ac64ad33d..ad55e1410 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -107,6 +107,7 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_file2(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 73827b7cf..fe22f6177 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -104,6 +104,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"file2", test_file2, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, -- cgit v1.2.3 From 4e6e40e5d850f737b25d657630ac89ad1d54e67c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:54:39 +0100 Subject: Fix up ADC tests command to read all channels at once --- src/systemcmds/tests/test_adc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 030ac6e23..7302f63cc 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of eight channels */ - struct adc_msg_s data[8]; + /* make space for a maximum of ten channels */ + struct adc_msg_s data[10]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); -- cgit v1.2.3 From d940bdf7f64268d46503cf4b0b996bec3036603e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 17:02:10 +0100 Subject: Consistently making space for a maximum of 12 channels, 10 channels (including reference / temperature) are used / available right now --- src/drivers/stm32/adc/adc.cpp | 2 +- src/modules/sensors/sensors.cpp | 4 ++-- src/modules/uORB/topics/sensor_combined.h | 4 ++-- src/systemcmds/tests/test_adc.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b8..0b8a275e6 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6560dec3d..8da5bb294 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,8 +1169,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of ten channels (to ensure reading all channels at once) */ - struct adc_msg_s buf_adc[10]; + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 918389f38..117105470 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[9]; /**< Channel indices of each of these values */ + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7302f63cc..03391b851 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of ten channels */ - struct adc_msg_s data[10]; + /* make space for a maximum of twelve channels */ + struct adc_msg_s data[12]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); -- cgit v1.2.3 From aa2b125a67c4b4d64b8c4b5bed662cdac1e5f1c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 May 2014 14:19:39 +0200 Subject: perf: Allow printing to arbritrary fds --- src/modules/systemlib/perf_counter.c | 16 +++++++++++----- src/modules/systemlib/perf_counter.h | 14 ++++++++++++-- src/systemcmds/perf/perf.c | 2 +- src/systemcmds/tests/tests_main.c | 2 +- 4 files changed, 25 insertions(+), 9 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index b4ca0ed3e..22182e39e 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -281,13 +281,19 @@ perf_reset(perf_counter_t handle) void perf_print_counter(perf_counter_t handle) +{ + perf_print_counter_fd(0, handle); +} + +void +perf_print_counter_fd(int fd, perf_counter_t handle) { if (handle == NULL) return; switch (handle->type) { case PC_COUNT: - printf("%s: %llu events\n", + dprintf(fd, "%s: %llu events\n", handle->name, ((struct perf_ctr_count *)handle)->event_count); break; @@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, @@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, @@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle) } void -perf_print_all(void) +perf_print_all(int fd) { perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); while (handle != NULL) { - perf_print_counter(handle); + perf_print_counter_fd(fd, handle); handle = (perf_counter_t)sq_next(&handle->link); } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index d8f69fdbf..6835ee4a2 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -121,16 +121,26 @@ __EXPORT extern void perf_cancel(perf_counter_t handle); __EXPORT extern void perf_reset(perf_counter_t handle); /** - * Print one performance counter. + * Print one performance counter to stdout * * @param handle The counter to print. */ __EXPORT extern void perf_print_counter(perf_counter_t handle); +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); + /** * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout */ -__EXPORT extern void perf_print_all(void); +__EXPORT extern void perf_print_all(int fd); /** * Reset all of the performance counters. diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b69ea597b..b08a2e3b7 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -73,7 +73,7 @@ int perf_main(int argc, char *argv[]) return -1; } - perf_print_all(); + perf_print_all(0 /* stdout */); fflush(stdout); return 0; } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index fe22f6177..e3f26924f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -235,7 +235,7 @@ test_perf(int argc, char *argv[]) printf("perf: expect count of 1\n"); perf_print_counter(ec); printf("perf: expect at least two counters\n"); - perf_print_all(); + perf_print_all(0); perf_free(cc); perf_free(ec); -- cgit v1.2.3 From 905299884db03a680f71d5ab6301596fa50ece9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 00:15:56 +0200 Subject: mtd cmd: Warnings eleminated --- src/systemcmds/tests/test_mtd.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index bac9efbdb..cdb4362ba 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -57,6 +57,8 @@ #define PARAM_FILE_NAME "/fs/mtd_params" static int check_user_abort(int fd); +static void print_fail(void); +static void print_success(void); int check_user_abort(int fd) { /* check if user wants to abort */ @@ -126,7 +128,7 @@ test_mtd(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -137,11 +139,14 @@ test_mtd(int argc, char *argv[]) int fd = open(PARAM_FILE_NAME, O_RDONLY); int rret = read(fd, read_buf, chunk_sizes[c]); close(fd); + if (rret <= 0) { + err(1, "read error"); + } fd = open(PARAM_FILE_NAME, O_WRONLY); printf("printing 2 percent of the first chunk:\n"); - for (int i = 0; i < sizeof(read_buf) / 50; i++) { + for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) { printf("%02X", read_buf[i]); } printf("\n"); @@ -171,9 +176,9 @@ test_mtd(int argc, char *argv[]) /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, chunk_sizes[c]); + int rret2 = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret2 != (int)chunk_sizes[c]) { warnx("READ ERROR!"); print_fail(); return 1; @@ -182,7 +187,7 @@ test_mtd(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d", j); print_fail(); @@ -211,7 +216,7 @@ test_mtd(int argc, char *argv[]) char ffbuf[64]; memset(ffbuf, 0xFF, sizeof(ffbuf)); int fd = open(PARAM_FILE_NAME, O_WRONLY); - for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) { int ret = write(fd, ffbuf, sizeof(ffbuf)); if (ret != sizeof(ffbuf)) { -- cgit v1.2.3 From d5b5dcef24a7f5c7b2ed98081dd69a05f8d46959 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:01:43 -0700 Subject: Fix bugs found through compiler warnings --- src/drivers/mkblctrl/mkblctrl.cpp | 8 ++++---- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 10 ++++++++-- src/modules/commander/commander.cpp | 1 + src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- src/modules/gpio_led/gpio_led.c | 3 ++- src/modules/px4iofirmware/sbus.c | 10 ++++++++-- src/systemcmds/mtd/mtd.c | 8 ++++++-- src/systemcmds/tests/test_mixer.cpp | 1 + 10 files changed, 33 insertions(+), 14 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..493678ae5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..b0a8527c2 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1535967b1..3cd250458 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..f8b9e9fbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt posNED[2] = -(hgt - hgtRef); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..69d8cfce8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..1fc6d1295 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - warnx("stop"); + // FIXME: Is this correct? changed from warnx + errx(0, "stop"); } else { errx(1, "not running"); diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..8ed69678c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,19 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..8b43e54da 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..dc2c20e79 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int -- cgit v1.2.3 From 2448e2adbc22af348aa799473c30ac5f1cba9cd0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:55:55 -0700 Subject: More tab fixes --- src/drivers/roboclaw/RoboClaw.cpp | 8 ++++---- src/modules/commander/commander.cpp | 2 +- src/modules/gpio_led/gpio_led.c | 4 ++-- src/systemcmds/mtd/mtd.c | 12 ++++++------ src/systemcmds/tests/test_mixer.cpp | 2 +- 5 files changed, 14 insertions(+), 14 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b0a8527c2..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -183,8 +183,8 @@ float RoboClaw::getMotorPosition(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Position; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } @@ -195,8 +195,8 @@ float RoboClaw::getMotorSpeed(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Speed; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3cd250458..aa243d79b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,7 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } - return true; + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index def958c95..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,13 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); - exit(0); + exit(0); } } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - exit(0); + exit(0); } else { errx(1, "not running"); } diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8b43e54da..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,12 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) { - // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried - // that but setting the bug speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to warnx. - warnx("failed to set bus speed"); - } + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc2c20e79..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,7 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); - return 0; + return 0; } static int -- cgit v1.2.3 From 92adbe9216c96c53d1baa4eb1e14b4ede272c080 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 17:47:24 -0700 Subject: Fix compiler warnings --- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 4 +-- src/drivers/gps/gps.cpp | 6 ++-- src/drivers/hmc5883/hmc5883.cpp | 4 +-- src/drivers/mkblctrl/mkblctrl.cpp | 19 ++++------ src/drivers/px4fmu/fmu.cpp | 12 +++---- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/examples/px4_simple_app/px4_simple_app.c | 1 + src/lib/geo/geo.c | 33 +++++++++-------- src/lib/geo_lookup/geo_mag_declination.c | 31 +++++++--------- .../attitude_estimator_ekf_params.c | 2 +- .../attitude_estimator_so3_main.cpp | 21 +++-------- .../commander/accelerometer_calibration.cpp | 6 ++-- src/modules/commander/calibration_routines.cpp | 2 +- src/modules/dataman/dataman.c | 3 +- .../ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/fw_pos_control_l1/landingslope.cpp | 16 ++++----- src/modules/fw_pos_control_l1/landingslope.h | 8 ++--- src/modules/mavlink/mavlink_main.cpp | 6 ++-- src/modules/mavlink/mavlink_main.h | 4 +-- src/modules/mavlink/mavlink_orb_subscription.cpp | 6 ++-- src/modules/mavlink/mavlink_stream.cpp | 6 +++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 +- .../position_estimator_inav_main.c | 41 ++++++++++------------ src/modules/px4iofirmware/protocol.h | 2 +- src/modules/px4iofirmware/px4io.c | 5 ++- src/modules/px4iofirmware/registers.c | 3 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/hx_stream.c | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 4 +-- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/param/param.c | 2 -- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 - src/systemcmds/mtd/mtd.c | 30 ++++++++-------- src/systemcmds/param/param.c | 4 +-- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 7 ++-- src/systemcmds/tests/test_file2.c | 10 +++--- src/systemcmds/tests/test_float.c | 10 +++--- src/systemcmds/tests/test_mathlib.cpp | 4 --- src/systemcmds/tests/test_mixer.cpp | 33 ++++++----------- src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 1 + src/systemcmds/tests/test_servo.c | 1 + 45 files changed, 162 insertions(+), 206 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index a856ccb02..3c05bfa46 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -54,7 +54,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 2de7063ea..1a7e068fe 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -364,7 +364,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -389,7 +389,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 8560716e9..dd505abdb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -357,7 +357,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -449,7 +449,7 @@ GPS::print_info() if (_report.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); @@ -578,7 +578,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e..b7b368a5e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), + _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -1228,7 +1228,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)1.0/_range_scale, (double)_range_ga); + (double)(1.0f/_range_scale), (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1c81910f6..3996b76a6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) : _task(-1), _t_actuators(-1), _t_actuator_armed(-1), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), _t_outputs(0), _t_esc_status(0), _num_outputs(0), + _primary_pwm_device(false), _motortest(false), _overrideSecurityChecks(false), - _motor(-1), - _px4mode(MAPPING_MK), - _frametype(FRAME_PLUS), - _primary_pwm_device(false), _task_should_exit(false), _armed(false), _mixers(nullptr) @@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) void MK::task_main() { - long update_rate_in_us = 0; - float tmpVal = 0; - /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -483,7 +480,6 @@ MK::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - update_rate_in_us = long(1000000 / _update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { @@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val) _retries = 0; uint8_t result[3] = { 0, 0, 0 }; uint8_t msg[2] = { 0, 0 }; - uint8_t rod = 0; uint8_t bytesToSendBL2 = 2; tmpVal = val; @@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val) if (debugCounter == 2000) { debugCounter = 0; - for (int i = 0; i < _num_outputs; i++) { + for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } @@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned motors, char *device_path) +mk_start(unsigned motors, const char *device_path) { int ret; @@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; - char *devicepath = ""; + const char *devicepath = ""; /* * optional parameters diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 84ea9a3bc..0a4635728 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -240,8 +240,6 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _control_subs({-1}), - _poll_fds_num(0), _armed_sub(-1), _outputs_pub(-1), _num_outputs(0), @@ -252,10 +250,12 @@ PX4FMU::PX4FMU() : _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _control_subs{-1}, + _poll_fds_num(0), + _failsafe_pwm{0}, + _disarmed_pwm{0}, + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 3b210ac59..c39494fb0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9109af14f..bca1715fa 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -616,7 +616,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 44e6dc7f3..4e9f099ed 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..212e33ff8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ @@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; - *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; - *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel @@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); @@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } if (arc_sweep >= 0) { @@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // as this function generally will not be called repeatedly when we are out of the sector. // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float start_disp_x = radius * sinf(arc_start_bearing); + float start_disp_y = radius * cosf(arc_start_bearing); + float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0f; + float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = lon_now + end_disp_x / 111111.0f; + float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now double d_lat = x_rad - current_x_rad; double d_lon = y_rad - current_y_rad; - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); double c = 2 * atan2(sqrt(a), sqrt(1 - a)); float dxy = CONSTANTS_RADIUS_OF_EARTH * c; diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index 09eac38f4..c41d52606 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -54,24 +54,19 @@ static const int8_t declination_table[13][37] = \ { - 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ - -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ - -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ - 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ - -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ - 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ - 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ - -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ - -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ - 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ - 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ - 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ - 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ - 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ - -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ - 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ - 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ - 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 49a892609..bc0e3b93a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI / 180.0f; + p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e55b01160..e49027e47 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl */ int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - //! Time constant float dt = 0.005f; @@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ @@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { @@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { - update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { - update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { - update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_start = hrt_absolute_time(); - // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], @@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7180048ff..24da452b1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { + int fd; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { @@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd) int res = OK; /* reset all offsets to zero and all scales to one */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd) if (res == OK) { /* apply new scaling and offsets */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index be38ea104..9d79124e7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], float aA, aB, aC, nA, nB, nC, dA, dB, dC; //Iterate N times, ignore stop condition. - int n = 0; + unsigned int n = 0; while (n < max_iterations) { n++; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..406293bda 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -50,6 +50,7 @@ #include #include "dataman.h" +#include /** * data manager app start / stop handling function @@ -187,7 +188,7 @@ create_work_item(void) if (item) { item->first = 1; lock_queue(&g_free_q); - for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 7aad849f9..15ceb57c0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -234,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); +void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 8ce465fe8..42e00da05 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,16 +46,16 @@ #include #include -void Landingslope::update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt) +void Landingslope::update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new) { - _landing_slope_angle_rad = landing_slope_angle_rad; - _flare_relative_alt = flare_relative_alt; - _motor_lim_relative_alt = motor_lim_relative_alt; - _H1_virt = H1_virt; + _landing_slope_angle_rad = landing_slope_angle_rad_new; + _flare_relative_alt = flare_relative_alt_new; + _motor_lim_relative_alt = motor_lim_relative_alt_new; + _H1_virt = H1_virt_new; calculateSlopeValues(); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index b54fd501c..a5975ad43 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -123,10 +123,10 @@ public: float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); - void update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt); + void update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a5e31ef4..43acee96f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; @@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6777d56c3..5f4117ae5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,9 +123,9 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); static int instance_count(); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,10 +47,10 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { } diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5ec30bd33..ed6776e5c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,7 +43,11 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : + _last_sent(0), + _channel(MAVLINK_COMM_0), + _interval(1000000), + next(nullptr) { } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 09960d106..c24c172af 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -545,7 +545,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -862,7 +861,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ - if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 62cee145e..9870ebd05 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -179,15 +179,21 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", + hrt_absolute_time(), msg, (double)dt, + (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], + (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + (double)acc[0], (double)acc[1], (double)acc[2], + (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], + (double)w_xy_gps_p, (double)w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -261,9 +267,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame @@ -285,7 +288,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) - hrt_abstime xy_src_time = 0; // time of last available position data bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid @@ -370,8 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); + warnx("baro offs: %.2f", (double)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -475,7 +477,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -497,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); } } else { @@ -510,7 +512,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ @@ -558,7 +560,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1 / w_flow; + eph_flow = 0.1f / w_flow; flow_valid = true; @@ -661,8 +663,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); } } @@ -746,10 +748,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { - eph *= 1.0 + dt; + eph *= 1.0f + dt; } if (epv < max_eph_epv) { - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ @@ -758,11 +760,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - /* try to estimate position during some time after position sources lost */ - if (use_gps_xy || use_flow) { - xy_src_time = t; - } - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 7471faec7..d5a6b1ec4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -312,7 +312,7 @@ struct IOPacket { #define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) static const uint8_t crc8_tab[256] __attribute__((unused)) = { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d4c25911e..cd134ccb4 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -37,6 +37,7 @@ */ #include +#include #include // required for task_create #include @@ -303,14 +304,12 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)minfo.mxordblk); + (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index db1836f4a..50108ea1b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] = [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; @@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16fcb4c26..8908adf4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -649,7 +649,7 @@ Sensors::parameters_update() if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8e9c2bfcf..52ae77de5 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -63,7 +63,7 @@ struct hx_stream { /* TX state */ int fd; bool tx_error; - uint8_t *tx_buf; + const uint8_t *tx_buf; unsigned tx_resid; uint32_t tx_crc; enum { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 092c0e2b0..4b22a46d0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; - const char *end = buf + buflen; /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { @@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float scale_yaw = 1.0f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) } /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0) { + if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 695574fdc..0548a9f7d 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -133,7 +133,7 @@ int lock_otp(void) // COMPLETE, BUSY, or other flash error? -int F_GetStatus(void) +static int F_GetStatus(void) { int fs = F_COMPLETE; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 7a499ca72..e44e6cdb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_sem = { .semcount = 1 }; - /** lock the parameter store */ static void param_lock(void) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index ea5ba9e52..c733a53d7 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } unsigned progress; - uint16_t temp_pwm; /* then set effective_pwm based on state */ switch (limit->state) { diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index fcc9b8366..a925cdd40 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -91,7 +91,7 @@ static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); -static void mtd_print_info(); +static void mtd_print_info(void); static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); @@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0; static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); +static void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + int mtd_main(int argc, char *argv[]) { if (argc >= 2) { @@ -355,7 +365,7 @@ static ssize_t mtd_get_partition_size(void) return partsize; } -void mtd_print_info() +void mtd_print_info(void) { if (!attached) exit(1); @@ -385,16 +395,6 @@ mtd_test(void) exit(1); } -void -mtd_status(void) -{ - if (!attached) - errx(1, "MTD driver not started"); - - mtd_print_info(); - exit(0); -} - void mtd_erase(char *partition_names[], unsigned n_partitions) { @@ -428,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) uint8_t v[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; + ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); if (fd == -1) { @@ -459,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) uint8_t v[128], v2[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; - off_t offset = 0; + ssize_t count = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); if (fd == -1) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index d92ee88ef..328205e29 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); -static void do_reset(); +static void do_reset(void); int param_main(int argc, char *argv[]) @@ -416,7 +416,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons) } static void -do_reset() +do_reset(void) { param_reset_all(); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 50dece816..0ecdc8eb9 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 96be1e8df..86f23b485 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -102,7 +102,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { @@ -116,7 +116,7 @@ test_file(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -224,9 +224,6 @@ test_file(int argc, char *argv[]) return 1; } - /* compare value */ - bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index ef555f6c3..8db3ea5ae 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -49,6 +49,8 @@ #include #include +#include "tests.h" + #define FLAG_FSYNC 1 #define FLAG_LSEEK 2 @@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t buffer[j] = get_value(ofs); ofs++; } - if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { - printf("write failed at offset %u\n", ofs); - exit(1); + if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { + printf("write failed at offset %u\n", ofs); + exit(1); } if (flags & FLAG_FSYNC) { fsync(fd); @@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t printf("read ofs=%u\r", ofs); } counter++; - if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); exit(1); } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 4921c9bbb..e33cc6ef7 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -94,7 +94,7 @@ int test_float(int argc, char *argv[]) printf("\t success: asinf(1.0f) == 1.57079f\n"); } else { - printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one); ret = -1; } @@ -128,7 +128,7 @@ int test_float(int argc, char *argv[]) float sinf_zero_one = sinf(0.1f); - if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { printf("\t success: sinf(0.1f) == 0.09983f\n"); } else { @@ -155,7 +155,7 @@ int test_float(int argc, char *argv[]) } char sbuf[30]; - sprintf(sbuf, "%8.4f", 0.553415f); + sprintf(sbuf, "%8.4f", (double)0.553415f); if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -166,7 +166,7 @@ int test_float(int argc, char *argv[]) ret = -5; } - sprintf(sbuf, "%8.4f", -0.553415f); + sprintf(sbuf, "%8.4f", (double)-0.553415f); if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -205,7 +205,7 @@ int test_float(int argc, char *argv[]) printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { - printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1); + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1); ret = -8; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 693a208ba..00c649c75 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -52,10 +52,6 @@ using namespace math; -const char* formatResult(bool res) { - return res ? "OK" : "ERROR"; -} - int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 0b826b826..8ab8fa2d6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[]) warnx("testing mixer"); - char *filename = "/etc/mixers/IO_pass.mix"; + const char *filename = "/etc/mixers/IO_pass.mix"; if (argc > 1) filename = argv[1]; @@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[]) * e.g. on PX4IO. */ - unsigned nused = 0; - const unsigned chunk_size = 64; MixerGroup mixer_group(mixer_callback, 0); @@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[]) return 1; /* FIRST mark the mixer as invalid */ - bool mixer_ok = false; /* THEN actually delete it */ mixer_group.reset(); char mixer_text[256]; /* large enough for one mixer */ @@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[]) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - bool mixer_ok = false; return 1; } @@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - mixer_ok = true; - } else { - /* not yet reached the end of the mixer, set as not ok */ - mixer_ok = false; - } - warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[]) should_arm = true; /* run through arming phase */ - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && @@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[]) return 1; } - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); sleepcount++; @@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = j/10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } @@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; @@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index b9041b013..addd57bea 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[]) int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 57c0e7f4c..c9f9ef439 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include "tests.h" diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ef8512bf5..9c6951ca2 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -51,6 +51,7 @@ #include #include +#include #include -- cgit v1.2.3 From ee8f4dcee639effe982bbb099a6f86f7c4fce5e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:44 +0200 Subject: systemcmds: Warning fixes --- src/systemcmds/mtd/24xxxx_mtd.c | 4 +++- src/systemcmds/pwm/pwm.c | 3 ++- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 3 ++- src/systemcmds/tests/test_float.c | 14 +++++++------- src/systemcmds/tests/test_mtd.c | 2 ++ 6 files changed, 17 insertions(+), 11 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index a3f9dffff..72200f418 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -237,8 +237,10 @@ void at24c_test(void) } else if (result != 1) { vdbg("unexpected %u\n", result); } - if ((count % 100) == 0) + + if ((count % 100) == 0) { vdbg("test %u errors %u\n", count, errors); + } } } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 1828c660f..e0e6ca537 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -515,7 +515,8 @@ pwm_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - int ret = read(0, &c, 1); + ret = read(0, &c, 1); + if (ret > 0) { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 0ecdc8eb9..fda949c61 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 86f23b485..570583dee 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -149,6 +149,8 @@ test_file(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -192,7 +194,6 @@ test_file(int argc, char *argv[]) warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index e33cc6ef7..bb8b6c7f5 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -68,7 +68,7 @@ int test_float(int argc, char *argv[]) float sinf_one = sinf(1.0f); float sqrt_two = sqrt(2.0f); - if (sinf_zero == 0.0f) { + if (fabsf(sinf_zero) < FLT_EPSILON) { printf("\t success: sinf(0.0f) == 0.0f\n"); } else { @@ -136,7 +136,7 @@ int test_float(int argc, char *argv[]) ret = -2; } - if (sqrt_two == 1.41421356f) { + if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) { printf("\t success: sqrt(2.0f) == 1.41421f\n"); } else { @@ -188,7 +188,7 @@ int test_float(int argc, char *argv[]) double d1d2 = d1 * d2; - if (d1d2 == 2.022200000000000219557705349871) { + if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) { printf("\t success: 1.0111 * 2.0 == 2.0222\n"); } else { @@ -201,7 +201,7 @@ int test_float(int argc, char *argv[]) // Assign value of f1 to d1 d1 = f1; - if (f1 == (float)d1) { + if (fabsf(f1 - (float)d1) < FLT_EPSILON) { printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { @@ -216,7 +216,7 @@ int test_float(int argc, char *argv[]) double sin_one = sin(1.0); double atan2_ones = atan2(1.0, 1.0); - if (sin_zero == 0.0) { + if (fabs(sin_zero - 0.0) < DBL_EPSILON) { printf("\t success: sin(0.0) == 0.0\n"); } else { @@ -224,7 +224,7 @@ int test_float(int argc, char *argv[]) ret = -9; } - if (sin_one == 0.841470984807896504875657228695) { + if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) { printf("\t success: sin(1.0) == 0.84147098480\n"); } else { @@ -232,7 +232,7 @@ int test_float(int argc, char *argv[]) ret = -10; } - if (atan2_ones != 0.785398) { + if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) { printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); } else { diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index cdb4362ba..43231ccad 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -171,6 +171,8 @@ test_mtd(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open(PARAM_FILE_NAME, O_RDONLY); -- cgit v1.2.3 From 2a79689224a381cf777c555d330f0a2aca8e3d9a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:51:06 -0700 Subject: Fix unused variable warnings --- src/modules/commander/commander.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 3 +++ 5 files changed, 8 insertions(+), 10 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f83c1caf8..3d4a297f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -773,7 +773,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e835cf81..3d6c62434 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -807,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -945,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Calculate altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..2e37d166e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..55a472bf6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 00c649c75..df3ddb966 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> %% Vector<3>", v1 % v2); TEST_OP("Vector<3> length", v1.length()); TEST_OP("Vector<3> length squared", v1.length_squared()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); +#pragma GCC diagnostic pop TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } -- cgit v1.2.3 From 680ebf29c3f8298e63d4c4d9c9076464e6f4b3c1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 15:11:46 -0700 Subject: Fix compiler warnings --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++++- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 +----- src/modules/navigator/mission.cpp | 8 ++++---- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/uORB/topics/telemetry_status.h | 10 ++++++++++ src/systemcmds/param/param.c | 1 - src/systemcmds/tests/test_mathlib.cpp | 2 ++ 7 files changed, 24 insertions(+), 13 deletions(-) (limited to 'src/systemcmds/tests') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 808b635bb..d8d45d34e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -139,7 +139,11 @@ ARCHWARNINGS = -Wall \ -Werror=format-security \ -Werror=array-bounds \ -Wfatal-errors \ - -Wformat=1 + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=unused-variable \ + -Werror=double-promotion \ + -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 973de1382..f33a1d780 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1742,7 +1742,6 @@ void AttPosEKF::FuseOptFlow() static float vd = 0.0f; static float pd = 0.0f; static float ptd = 0.0f; - static Vector3f delAng; static float R_LOS = 0.01f; static float losPred[2]; @@ -1820,9 +1819,6 @@ void AttPosEKF::FuseOptFlow() // calculate relative velocity in sensor frame relVelSensor = Tns*velNED_local; - // calculate delta angles in sensor axes - Vector3f delAngRel = Tbs*delAng; - // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes losPred[0] = relVelSensor.y/range; losPred[1] = -relVelSensor.x/range; @@ -1959,7 +1955,7 @@ void AttPosEKF::FuseOptFlow() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cf6a764bf..d39ca6cf9 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,11 +65,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), + _need_takeoff(true), + _takeoff(false), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE), - _need_takeoff(true), - _takeoff(false) + _mission_type(MISSION_TYPE_NONE) { /* load initial params */ updateParams(); @@ -334,7 +334,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a1c6f09d0..4adf77dce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -57,10 +57,10 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), + _mission_item({0}), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item({0}) + _time_first_inside_orbit(0) { } diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index c4b99d520..3347724a5 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -86,4 +86,14 @@ static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_I ORB_ID(telemetry_status_3), }; +// This is a hack to quiet an unused-variable warning for when telemetry_status.h is +// included but telemetry_status_orb_id is not referenced. The inline works if you +// choose to use it, but you can continue to just directly index into the array as well. +// If you don't use the inline this ends up being a no-op with no additional code emitted. +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); +extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) +{ + return telemetry_status_orb_id[index]; +} + #endif /* TOPIC_TELEMETRY_STATUS_H */ diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 235ab7186..28e1b108b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -231,7 +231,6 @@ do_show_print(void *arg, param_t param) /* start search */ const char *ss = search_string; const char *pp = p_name; - bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ while (*ss != '\0' && *pp != '\0') { diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index df3ddb966..70d173fc9 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -98,6 +98,8 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> length squared", v1.length_squared()); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); #pragma GCC diagnostic pop -- cgit v1.2.3 From 0054eb23d857844ebae34a7d198fc60e538ccd3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Jul 2014 15:44:22 +0200 Subject: Update sensors tests --- src/systemcmds/tests/test_sensors.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 096106ff3..a4f17eebd 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -139,7 +139,14 @@ accel(int argc, char *argv[]) } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("ACCEL1 acceleration values out of range!"); + warnx("ACCEL acceleration values out of range!"); + return ERROR; + } + + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL scale error!"); return ERROR; } @@ -186,6 +193,13 @@ accel1(int argc, char *argv[]) return ERROR; } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 8.0f || len > 12.0f) { + warnx("ACCEL1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: ACCEL1 passed all tests successfully\n"); @@ -225,6 +239,13 @@ gyro(int argc, char *argv[]) printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); close(fd); @@ -263,6 +284,13 @@ gyro1(int argc, char *argv[]) printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len > 0.3f) { + warnx("GYRO1 scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: GYRO1 passed all tests successfully\n"); close(fd); @@ -301,6 +329,13 @@ mag(int argc, char *argv[]) printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } + float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); + + if (len < 1.0f || len > 3.0f) { + warnx("MAG scale error!"); + return ERROR; + } + /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); close(fd); -- cgit v1.2.3 From b288b010f12213a4388b627bce7fd6cb4cdedea5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 18:42:00 +0200 Subject: tests, drive by: Fix double comparison, use reasonable margin based on context of test --- src/systemcmds/tests/test_bson.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/systemcmds/tests') diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 6130fe763..12d598df4 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } - if (node->d != sample_double) { + if (fabs(node->d - sample_double) > 1e-12) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } -- cgit v1.2.3