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') 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') 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') 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') 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') 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') 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') 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') 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') 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') 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 40196275d0e899eac49731d0c2b1dda96cafb84f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 15:23:54 +0100 Subject: Compile warning fixes on preflight check --- src/systemcmds/preflight_check/preflight_check.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 982b03782..86e4ff545 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -87,9 +88,7 @@ int preflight_check_main(int argc, char *argv[]) /* give the system some time to sample the sensors in the background */ usleep(150000); - /* ---- MAG ---- */ - close(fd); fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); -- 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') 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 3392086f8f317823061363296c8d3e18be4ee88e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 21:19:25 +0100 Subject: Mixer load: don't upload empty mixers from non-existing files --- src/modules/systemlib/mixer/mixer_load.c | 6 ++++-- src/systemcmds/mixer/mixer.cpp | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) (limited to 'src/systemcmds') diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index a55ddf8a3..b05273c0d 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); if (fp == NULL) { - return 1; + warnx("file not found"); + return -1; } /* read valid lines from the file into a buffer */ @@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - return 1; + warnx("line too long"); + return -1; } /* add the line to the buffer */ diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 6da39d371..2f5ed3265 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,8 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0], sizeof(buf)); + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) + err(1, "can't load mixer: %s", fname); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); -- 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') 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 f8187650083b361f61c07ceb712c21a25d1c80c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:32:55 +0400 Subject: top: CPU % indication bug fixed --- src/systemcmds/top/top.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 1ca3fc928..37e913040 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -233,8 +233,8 @@ top_main(void) system_load.tasks[i].tcb->pid, CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), stack_size - stack_free, stack_size, system_load.tasks[i].tcb->sched_priority, -- 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') 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 cb8bd1a3ad21fec45af7a1f1d53fe7b891c59c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 21:05:13 +0400 Subject: dumpfile command and fetch_log.py tool implemented to fetch logs via nsh console on USB --- Tools/fetch_log.py | 133 ++++++++++++++++++++++++++++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/dumpfile/dumpfile.c | 116 +++++++++++++++++++++++++++++ src/systemcmds/dumpfile/module.mk | 41 +++++++++++ 5 files changed, 292 insertions(+) create mode 100644 Tools/fetch_log.py create mode 100644 src/systemcmds/dumpfile/dumpfile.c create mode 100644 src/systemcmds/dumpfile/module.mk (limited to 'src/systemcmds') diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py new file mode 100644 index 000000000..edcc6557c --- /dev/null +++ b/Tools/fetch_log.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Log fetcher +# +# Print list of logs: +# python fetch_log.py +# +# Fetch log: +# python fetch_log.py sess001/log001.bin +# + +import serial, time, sys, os + +def wait_for_string(ser, s, timeout=1.0, debug=False): + t0 = time.time() + buf = [] + res = [] + n = 0 + while (True): + c = ser.read() + if debug: + sys.stderr.write(c) + buf.append(c) + if len(buf) > len(s): + res.append(buf.pop(0)) + n += 1 + if n % 10000 == 0: + sys.stderr.write(str(n) + "\n") + if "".join(buf) == s: + break + if timeout > 0.0 and time.time() - t0 > timeout: + raise Exception("Timeout while waiting for: " + s) + return "".join(res) + +def exec_cmd(ser, cmd, timeout): + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout) + return wait_for_string(ser, "nsh> \x1b[K", timeout) + +def ls_dir(ser, dir, timeout=1.0): + res = [] + for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]: + res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) + return res + +def list_logs(ser): + logs_dir = "/fs/microsd/log" + res = [] + for d in ls_dir(ser, logs_dir): + if d[2]: + sess_dir = d[0][:-1] + for f in ls_dir(ser, logs_dir + "/" + sess_dir): + log_file = f[0] + log_size = f[1] + res.append(sess_dir + "/" + log_file + "\t" + str(log_size)) + return "\n".join(res) + +def fetch_log(ser, fn, timeout): + cmd = "dumpfile " + fn + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout, True) + res = wait_for_string(ser, "\n", timeout, True) + data = [] + if res.startswith("OK"): + size = int(res.split()[1]) + n = 0 + print "Reading data:" + while (n < size): + buf = ser.read(min(size - n, 8192)) + data.append(buf) + n += len(buf) + sys.stdout.write(".") + sys.stdout.flush() + print + else: + raise Exception("Error reading log") + wait_for_string(ser, "nsh> \x1b[K", timeout) + return "".join(data) + +def main(): + dev = "/dev/tty.usbmodem1" + ser = serial.Serial(dev, "115200", timeout=0.2) + if len(sys.argv) < 2: + print list_logs(ser) + else: + log_file = sys.argv[1] + data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0) + try: + os.mkdir(log_file.split("/")[0]) + except: + pass + fout = open(log_file, "wb") + fout.write(data) + fout.close() + ser.close() + +if __name__ == "__main__": + main() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index aff614cbb..651c2ac38 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -56,6 +56,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..889abc4d9 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,6 +63,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/src/systemcmds/dumpfile/dumpfile.c b/src/systemcmds/dumpfile/dumpfile.c new file mode 100644 index 000000000..c18814342 --- /dev/null +++ b/src/systemcmds/dumpfile/dumpfile.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 dumpfile.c + * + * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout. + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +__EXPORT int dumpfile_main(int argc, char *argv[]); + +int +dumpfile_main(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "usage: dumpfile "); + } + + /* open input file */ + FILE *f; + f = fopen(argv[1], "r"); + + if (f == NULL) { + printf("ERROR\n"); + exit(1); + } + + /* get file size */ + fseek(f, 0L, SEEK_END); + int size = ftell(f); + fseek(f, 0L, SEEK_SET); + + printf("OK %d\n", size); + + /* configure stdout */ + int out = fileno(stdout); + + struct termios tc; + struct termios tc_old; + tcgetattr(out, &tc); + + /* save old terminal attributes to restore it later on exit */ + memcpy(&tc_old, &tc, sizeof(tc)); + + /* don't add CR on each LF*/ + tc.c_oflag &= ~ONLCR; + + if (tcsetattr(out, TCSANOW, &tc) < 0) { + warnx("ERROR setting stdout attributes"); + exit(1); + } + + char buf[512]; + int nread; + + /* dump file */ + while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) { + if (write(out, buf, nread) <= 0) { + warnx("error dumping file"); + break; + } + } + + fsync(out); + fclose(f); + + /* restore old terminal attributes */ + if (tcsetattr(out, TCSANOW, &tc_old) < 0) { + warnx("ERROR restoring stdout attributes"); + exit(1); + } + + return OK; +} diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk new file mode 100644 index 000000000..36461f477 --- /dev/null +++ b/src/systemcmds/dumpfile/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Dump file utility +# + +MODULE_COMMAND = dumpfile +SRCS = dumpfile.c + +MAXOPTIMIZATION = -Os -- 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') 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') 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 267b78f072b105294e28578d2d8bf28e56ff9fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 21:31:30 +0200 Subject: Fix of errors triggered by more pedantic compile options --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- .../fw_att_pos_estimator_main.cpp | 6 +++--- src/modules/sensors/sensors.cpp | 22 +++++++++++----------- src/systemcmds/config/config.c | 2 +- 4 files changed, 16 insertions(+), 16 deletions(-) (limited to 'src/systemcmds') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 705e98eea..3cb9abc49 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1015,7 +1015,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): /* copy the current output value from the channel */ - *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue; break; diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea611802..46bd399f7 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -732,21 +732,21 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 3: { const char* str = "switching dynamic / static state"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4083b8b4f..6258a18b1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -635,39 +635,39 @@ Sensors::parameters_update() /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -737,12 +737,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 476015f3e..e6d4b763b 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -121,7 +121,7 @@ do_device(int argc, char *argv[]) errx(ret,"uORB publications could not be unblocked"); } else { - errx("no valid command: %s", argv[1]); + errx(1, "no valid command: %s", argv[1]); } } -- cgit v1.2.3 From 08e5ed5a1d630f840a838006bab6b1d1ae6aa6c1 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:14:12 +0200 Subject: added version nsh command, it can replace hw_ver sss --- makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/version/module.mk | 43 +++++++++++ src/systemcmds/version/version.c | 138 ++++++++++++++++++++++++++++++++++ 3 files changed, 182 insertions(+) create mode 100644 src/systemcmds/version/module.mk create mode 100644 src/systemcmds/version/version.c (limited to 'src/systemcmds') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc..7ec6a2fc6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,6 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/version # # General system control diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/version/module.mk new file mode 100644 index 000000000..3dac09239 --- /dev/null +++ b/src/systemcmds/version/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = version +SRCS = version.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c new file mode 100644 index 000000000..38730b10d --- /dev/null +++ b/src/systemcmds/version/version.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Vladimir Kulla + * + * 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 version.c + * + * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc + */ + +#include + +#include +#include +#include +#include + +__EXPORT int version_main(int argc, char *argv[]); + +static void usage(const char *reason) +{ + if (reason != NULL) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); +} + +static void version_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash:"); + } + printf("%s\n", FW_GIT); +} + +static void version_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +static void version_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +static void version_gcc(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used: "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} +static char sz_ver_hw_str[] = "hw"; + +int version_main(int argc, char *argv[]) +{ + if (argc >= 2) + { + if (argv[1] != NULL) + { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) + { + version_hwarch(0); + } + else if (!strncmp(argv[1], "git", 3)) + { + version_githash(0); + } + else if (!strncmp(argv[1], "date", 3)) + { + version_date(0); + } + else if (!strncmp(argv[1], "gcc", 3)) + { + version_gcc(0); + } + else if (!strncmp(argv[1], "all", 3)) + { + printf("Pixhawk version info\n"); + version_hwarch(1); + version_date(1); + version_githash(1); + version_gcc(1); + } + else + { + printf("unknown command: %s\n", argv[1]); + } + } + else + { + usage("Error, input parameter NULL.\n"); + } + } + else + { + usage("Error, not enough parameters."); + } + return OK; +} -- cgit v1.2.3 From fd95adc710cd04855b8f7c2abaf73d9220cd497e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:34:45 +0200 Subject: corrections before xmerge --- src/systemcmds/version/version.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c index 38730b10d..37a51dbfc 100644 --- a/src/systemcmds/version/version.c +++ b/src/systemcmds/version/version.c @@ -45,6 +45,13 @@ #include #include +static char sz_ver_hw_str[] = "hw"; +static char sz_ver_git_str[] = "git"; +static char sz_ver_date_str[] = "date"; +static char sz_ver_gcc_str[] = "gcc"; +static char sz_ver_all_str[] = "all"; + + __EXPORT int version_main(int argc, char *argv[]); static void usage(const char *reason) @@ -59,7 +66,7 @@ static void usage(const char *reason) static void version_githash(int bShowPrefix) { if (bShowPrefix == 1) { - printf("FW git-hash:"); + printf("FW git-hash: "); } printf("%s\n", FW_GIT); } @@ -88,7 +95,6 @@ static void version_gcc(int bShowPrefix) } printf("%s\n", __VERSION__); } -static char sz_ver_hw_str[] = "hw"; int version_main(int argc, char *argv[]) { @@ -100,19 +106,19 @@ int version_main(int argc, char *argv[]) { version_hwarch(0); } - else if (!strncmp(argv[1], "git", 3)) + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { version_githash(0); } - else if (!strncmp(argv[1], "date", 3)) + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { version_date(0); } - else if (!strncmp(argv[1], "gcc", 3)) + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { version_gcc(0); } - else if (!strncmp(argv[1], "all", 3)) + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { printf("Pixhawk version info\n"); version_hwarch(1); -- cgit v1.2.3 From 5ea1105451330aa55e0c331b99ba2ab60e6f8c15 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 15:12:05 +0200 Subject: changed dir from version to ver to keep it shorter added "hw_ver compare" as command option so we can replace hw_ver in future --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 +- src/systemcmds/ver/module.mk | 44 +++++++++ src/systemcmds/ver/ver.c | 175 ++++++++++++++++++++++++++++++++++ src/systemcmds/ver/ver.h | 64 +++++++++++++ src/systemcmds/version/module.mk | 43 --------- src/systemcmds/version/version.c | 144 ---------------------------- 7 files changed, 285 insertions(+), 188 deletions(-) create mode 100644 src/systemcmds/ver/module.mk create mode 100644 src/systemcmds/ver/ver.c create mode 100644 src/systemcmds/ver/ver.h delete mode 100644 src/systemcmds/version/module.mk delete mode 100644 src/systemcmds/version/version.c (limited to 'src/systemcmds') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d0..20e7ab331 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -57,6 +57,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7ec6a2fc6..bbb754eb0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,7 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile -MODULES += systemcmds/version +MODULES += systemcmds/ver # # General system control diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk new file mode 100644 index 000000000..2eeb80b61 --- /dev/null +++ b/src/systemcmds/ver/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# "version" nsh-command displays version infromation for hw,sw, gcc,build etc +# can be also included and used in own code via "ver.h" +# + +MODULE_COMMAND = ver +SRCS = ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c new file mode 100644 index 000000000..f44fd1afe --- /dev/null +++ b/src/systemcmds/ver/ver.c @@ -0,0 +1,175 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 ver.c +* +* Version command, unifies way of showing versions of HW, SW, Build, gcc +* In case you want to add new version just extend version_main function +* +* External use of the version functions is possible, include "vercmd.h" +* and use functions from header with prefix ver_ +* +* @author Vladimir Kulla +*/ + +#include +#include +#include +#include +#include + +#include "ver.h" + +// string constants for version commands +static const char sz_ver_hw_str[] = "hw"; +static const char sz_ver_hwcmp_str[]= "hwcmp"; +static const char sz_ver_git_str[] = "git"; +static const char sz_ver_date_str[] = "date"; +static const char sz_ver_gcc_str[] = "gcc"; +static const char sz_ver_all_str[] = "all"; + +static void usage(const char *reason) +{ + if (reason != NULL) { + printf("%s\n", reason); + } + + printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); +} + +void ver_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash: "); + } + printf("%s\n", FW_GIT); +} + +void ver_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +void ver_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +void ver_gcclong(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (long): "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} + +void ver_gccshort(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (short): "); + + } + printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); +} + +int ver_gccnum() +{ + return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; +} + +__EXPORT int ver_main(int argc, char *argv[]); + +int ver_main(int argc, char *argv[]) +{ + int ret = 1; //defaults to an error + + // first check if there are at least 2 params + if (argc >= 2) { + if (argv[1] != NULL) { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + ver_hwarch(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + if (argc >= 3 && argv[2] != NULL) { + // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + } else { + errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + } + } + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + ver_githash(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { + ver_date(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + ver_gcclong(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { + printf("Pixhawk version info\n"); + ver_hwarch(1); + ver_date(1); + ver_githash(1); + ver_gcclong(1); + ret = 0; + } + else { + errx(1, "unknown command.\n"); + } + } + else { + usage("Error, input parameter NULL.\n"); + } + } + else { + usage("Error, not enough parameters."); + } + + return ret; +} diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h new file mode 100644 index 000000000..3de308c05 --- /dev/null +++ b/src/systemcmds/ver/ver.h @@ -0,0 +1,64 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 vercmd.h +* +* Version command, unifies way of showing versions of hW, sW, build, gcc +* Mainly used from nsh-console, shall replace also hw_arch command +* +* External use: possible, include "vercmd.h" and use "ver_xx" functions +* +* Extension: possible, just add new else-if to version_main function +* +* @author Vladimir Kulla +*/ + +#ifndef VERCMD_H_ +#define VERCMD_H_ + + +void ver_githash(int bShowPrefix); + +void ver_hwarch(int bShowPrefix); + +void ver_date(int bShowPrefix); + +void ver_gcclong(int bShowPrefix); +void ver_gccshort(int bShowPrefix); +int ver_gccnum(); + + + + +#endif /* VERCMD_H_ */ diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/version/module.mk deleted file mode 100644 index 3dac09239..000000000 --- a/src/systemcmds/version/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 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. -# -############################################################################ - -# -# Show and test hardware version -# - -MODULE_COMMAND = version -SRCS = version.c - -MODULE_STACKSIZE = 1024 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c deleted file mode 100644 index 37a51dbfc..000000000 --- a/src/systemcmds/version/version.c +++ /dev/null @@ -1,144 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Vladimir Kulla - * - * 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 version.c - * - * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc - */ - -#include - -#include -#include -#include -#include - -static char sz_ver_hw_str[] = "hw"; -static char sz_ver_git_str[] = "git"; -static char sz_ver_date_str[] = "date"; -static char sz_ver_gcc_str[] = "gcc"; -static char sz_ver_all_str[] = "all"; - - -__EXPORT int version_main(int argc, char *argv[]); - -static void usage(const char *reason) -{ - if (reason != NULL) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); -} - -static void version_githash(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("FW git-hash: "); - } - printf("%s\n", FW_GIT); -} - -static void version_hwarch(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("HW arch: "); - } - printf("%s\n", HW_ARCH); -} - -static void version_date(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("Build date: "); - } - printf("%s %s\n", __DATE__, __TIME__); -} - -static void version_gcc(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used: "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); - } - printf("%s\n", __VERSION__); -} - -int version_main(int argc, char *argv[]) -{ - if (argc >= 2) - { - if (argv[1] != NULL) - { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) - { - version_hwarch(0); - } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) - { - version_githash(0); - } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) - { - version_date(0); - } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) - { - version_gcc(0); - } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) - { - printf("Pixhawk version info\n"); - version_hwarch(1); - version_date(1); - version_githash(1); - version_gcc(1); - } - else - { - printf("unknown command: %s\n", argv[1]); - } - } - else - { - usage("Error, input parameter NULL.\n"); - } - } - else - { - usage("Error, not enough parameters."); - } - return OK; -} -- cgit v1.2.3 From bd5a0cef1aad42f8deb1d58e573631436630246e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 17:42:45 +0200 Subject: ver command ready including hwcmp which replaces hw_ver, removed hw_ver updated all scripts to use new ver hwcmp command q --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 ++-- makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 2 +- src/systemcmds/hw_ver/hw_ver.c | 73 ----------------------------------- src/systemcmds/hw_ver/module.mk | 43 --------------------- src/systemcmds/ver/ver.c | 64 ++++++++++-------------------- src/systemcmds/ver/ver.h | 64 ------------------------------ 11 files changed, 29 insertions(+), 233 deletions(-) delete mode 100644 src/systemcmds/hw_ver/hw_ver.c delete mode 100644 src/systemcmds/hw_ver/module.mk delete mode 100644 src/systemcmds/ver/ver.h (limited to 'src/systemcmds') diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 6e8fdbc0f..e23aebd87 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ px4io recovery # Adjust PX4IO update rate limit # set PX4IO_LIMIT 400 -if hw_ver compare PX4FMU_V1 +if ver hwcmp PX4FMU_V1 then set PX4IO_LIMIT 200 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273..3469d5f5f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,7 +5,7 @@ if [ -d /fs/microsd ] then - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 8 -t diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 235be2431..1e14930fe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -22,7 +22,7 @@ then echo "[init] Using L3GD20(H)" fi -if hw_ver compare PX4FMU_V2 +if ver hwcmp PX4FMU_V2 then if lsm303d start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea5cf8deb..3fb5fb733 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -246,7 +246,7 @@ then echo "[init] ERROR: PX4IO not found, disabling output" # Avoid using ttyS0 for MAVLink on FMUv1 - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -260,7 +260,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -306,7 +306,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then @@ -381,7 +381,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 20e7ab331..61a417f30 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -55,7 +55,6 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bbb754eb0..65ca24325 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,7 +63,6 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d..84274bf75 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -29,7 +29,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver +MODULES += systemcmds/ver # # Library modules diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c deleted file mode 100644 index 4b84523cc..000000000 --- a/src/systemcmds/hw_ver/hw_ver.c +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 hw_ver.c - * - * Show and test hardware version. - */ - -#include - -#include -#include -#include -#include - -__EXPORT int hw_ver_main(int argc, char *argv[]); - -int -hw_ver_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "show")) { - printf(HW_ARCH "\n"); - exit(0); - } - - if (!strcmp(argv[1], "compare")) { - if (argc >= 3) { - int ret = strcmp(HW_ARCH, argv[2]) != 0; - if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); - } - exit(ret); - - } else { - errx(1, "not enough arguments, try 'compare PX4FMU_1'"); - } - } - } - - errx(1, "expected a command, try 'show' or 'compare'"); -} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk deleted file mode 100644 index 3cc08b6a1..000000000 --- a/src/systemcmds/hw_ver/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 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. -# -############################################################################ - -# -# Show and test hardware version -# - -MODULE_COMMAND = hw_ver -SRCS = hw_ver.c - -MODULE_STACKSIZE = 1024 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index f44fd1afe..c932bc162 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -34,28 +34,22 @@ /** * @file ver.c * -* Version command, unifies way of showing versions of HW, SW, Build, gcc +* Version command, unifies way of showing versions of HW, SW, Build, GCC * In case you want to add new version just extend version_main function * -* External use of the version functions is possible, include "vercmd.h" -* and use functions from header with prefix ver_ -* * @author Vladimir Kulla */ -#include #include #include -#include #include - -#include "ver.h" +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[]= "hwcmp"; static const char sz_ver_git_str[] = "git"; -static const char sz_ver_date_str[] = "date"; +static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; @@ -65,10 +59,10 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); } -void ver_githash(int bShowPrefix) +static void ver_githash(int bShowPrefix) { if (bShowPrefix == 1) { printf("FW git-hash: "); @@ -76,7 +70,7 @@ void ver_githash(int bShowPrefix) printf("%s\n", FW_GIT); } -void ver_hwarch(int bShowPrefix) +static void ver_hwarch(int bShowPrefix) { if (bShowPrefix == 1) { printf("HW arch: "); @@ -84,37 +78,22 @@ void ver_hwarch(int bShowPrefix) printf("%s\n", HW_ARCH); } -void ver_date(int bShowPrefix) +static void ver_bdate(int bShowPrefix) { if (bShowPrefix == 1) { - printf("Build date: "); + printf("Build datetime: "); } printf("%s %s\n", __DATE__, __TIME__); } -void ver_gcclong(int bShowPrefix) +static void ver_gcclong(int bShowPrefix) { if (bShowPrefix == 1) { - printf("GCC used (long): "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + printf("GCC toolchain: "); } printf("%s\n", __VERSION__); } -void ver_gccshort(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used (short): "); - - } - printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); -} - -int ver_gccnum() -{ - return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; -} - __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) @@ -124,37 +103,36 @@ int ver_main(int argc, char *argv[]) // first check if there are at least 2 params if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { ver_hwarch(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { // compare 3rd parameter with HW_ARCH string, in case of match, return 0 - ret = strcmp(HW_ARCH, argv[2]) != 0; + ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); + printf("hwver match: %s\n", HW_ARCH); } } else { - errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { ver_githash(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { - ver_date(0); + else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + ver_bdate(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { ver_gcclong(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { - printf("Pixhawk version info\n"); + else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { ver_hwarch(1); - ver_date(1); + ver_bdate(1); ver_githash(1); ver_gcclong(1); ret = 0; diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h deleted file mode 100644 index 3de308c05..000000000 --- a/src/systemcmds/ver/ver.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2012-2014 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 vercmd.h -* -* Version command, unifies way of showing versions of hW, sW, build, gcc -* Mainly used from nsh-console, shall replace also hw_arch command -* -* External use: possible, include "vercmd.h" and use "ver_xx" functions -* -* Extension: possible, just add new else-if to version_main function -* -* @author Vladimir Kulla -*/ - -#ifndef VERCMD_H_ -#define VERCMD_H_ - - -void ver_githash(int bShowPrefix); - -void ver_hwarch(int bShowPrefix); - -void ver_date(int bShowPrefix); - -void ver_gcclong(int bShowPrefix); -void ver_gccshort(int bShowPrefix); -int ver_gccnum(); - - - - -#endif /* VERCMD_H_ */ -- cgit v1.2.3 From a1e4435e163f4f01885de58dd492d7716863c05e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 30 Apr 2014 17:50:18 +0200 Subject: esc_calib: corrected name of mc controller --- src/systemcmds/esc_calib/esc_calib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index ad1996694..7d80af307 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -182,7 +182,7 @@ esc_calib_main(int argc, char *argv[]) if (orb_updated) { errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" - "\tmultirotor_att_control stop\n" + "\tmc_att_control stop\n" "\tfw_att_control stop\n"); } -- cgit v1.2.3 From a8743184c3d24e6ec8086602241096021d9aba84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 May 2014 16:02:00 +0200 Subject: Add command to do PWM step inputs --- src/systemcmds/pwm/pwm.c | 106 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 105 insertions(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 7c23f38c2..1828c660f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -443,6 +443,110 @@ pwm_main(int argc, char *argv[]) exit(0); } } + usleep(2000); + } + exit(0); + + + } else if (!strcmp(argv[1], "steps")) { + + if (set_mask == 0) { + usage("no channels set"); + } + + /* get current servo values */ + struct pwm_output_values last_spos; + + for (unsigned i = 0; i < servo_count; i++) { + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_GET(%d)", i); + } + + /* perform PWM output */ + + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now."); + sleep(5); + + unsigned off = 900; + unsigned idle = 1300; + unsigned full = 2000; + unsigned steps_timings_us[] = {2000, 5000, 20000, 50000}; + + unsigned phase = 0; + unsigned phase_counter = 0; + unsigned const phase_maxcount = 20; + + for ( unsigned steps_timing_index = 0; + steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]); + steps_timing_index++ ) { + + warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]); + + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + int ret = read(0, &c, 1); + if (ret > 0) { + /* reset output to the last value */ + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< phase_maxcount) { + phase++; + phase_counter = 0; + } + } } exit(0); -- cgit v1.2.3 From 85ac2796a0f6639b279ab0b7476c98c7a7d551a7 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Thu, 1 May 2014 23:36:35 +0200 Subject: simplified code, which is now less robust but smaller and easier to read (comment Babushkin) formated source code with fix_code_style.sh (comment Babushkin) fixed Copyright (comment Babushkin) --- src/systemcmds/ver/ver.c | 88 ++++++++++++++++-------------------------------- 1 file changed, 29 insertions(+), 59 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index c932bc162..9ae080ee2 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +* Copyright (c) 2014 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 @@ -47,7 +47,7 @@ // string constants for version commands static const char sz_ver_hw_str[] = "hw"; -static const char sz_ver_hwcmp_str[]= "hwcmp"; +static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; @@ -62,38 +62,6 @@ static void usage(const char *reason) printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); } -static void ver_githash(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("FW git-hash: "); - } - printf("%s\n", FW_GIT); -} - -static void ver_hwarch(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("HW arch: "); - } - printf("%s\n", HW_ARCH); -} - -static void ver_bdate(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("Build datetime: "); - } - printf("%s %s\n", __DATE__, __TIME__); -} - -static void ver_gcclong(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC toolchain: "); - } - printf("%s\n", __VERSION__); -} - __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) @@ -104,48 +72,50 @@ int ver_main(int argc, char *argv[]) if (argc >= 2) { if (argv[1] != NULL) { if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - ver_hwarch(0); + printf("%s\n", HW_ARCH); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + + } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { // compare 3rd parameter with HW_ARCH string, in case of match, return 0 ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); + if (ret == 0) { - printf("hwver match: %s\n", HW_ARCH); + printf("ver hwcmp match: %s\n", HW_ARCH); } + } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } - } - else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - ver_githash(0); + + } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("%s\n", FW_GIT); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - ver_bdate(0); + + } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + printf("%s %s\n", __DATE__, __TIME__); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - ver_gcclong(0); + + } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("%s\n", __VERSION__); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { - ver_hwarch(1); - ver_bdate(1); - ver_githash(1); - ver_gcclong(1); + + } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { + printf("HW arch: %s\n", HW_ARCH); + printf("Build datetime: %s %s\n", __DATE__, __TIME__); + printf("FW git-hash: %s\n", FW_GIT); + printf("GCC toolchain: %s\n", __VERSION__); ret = 0; - } - else { + + } else { errx(1, "unknown command.\n"); } - } - else { + + } else { usage("Error, input parameter NULL.\n"); } - } - else { + + } else { usage("Error, not enough parameters."); } -- cgit v1.2.3 From e4c0a224af5a2835e08786a2d3ed3e641cfcb22d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 20:20:29 +0200 Subject: Fix a param save issue where a state variable might preven the parameters from being saved (identified and fixed by ultrasystem) --- src/modules/systemlib/param/param.c | 64 ++----------------------------------- src/systemcmds/param/param.c | 18 +++++++++++ 2 files changed, 21 insertions(+), 61 deletions(-) (limited to 'src/systemcmds') diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2d773fd25..7a499ca72 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -521,73 +521,15 @@ param_save_default(void) return ERROR; } - if (res == OK) { - res = param_export(fd, false); + res = param_export(fd, false); - if (res != OK) { - warnx("failed to write parameters to file: %s", filename); - } + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); } close(fd); return res; - -#if 0 - const char *filename_tmp = malloc(strlen(filename) + 5); - sprintf(filename_tmp, "%s.tmp", filename); - - /* delete temp file if exist */ - res = unlink(filename_tmp); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete temp file: %s", filename_tmp); - - if (res == OK) { - /* write parameters to temp file */ - fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("failed to open temp file: %s", filename_tmp); - res = ERROR; - } - - if (res == OK) { - res = param_export(fd, false); - - if (res != OK) - warnx("failed to write parameters to file: %s", filename_tmp); - } - - close(fd); - } - - if (res == OK) { - /* delete parameters file */ - res = unlink(filename); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete parameters file: %s", filename); - } - - if (res == OK) { - /* rename temp file to parameters */ - res = rename(filename_tmp, filename); - - if (res != OK) - warn("failed to rename %s to %s", filename_tmp, filename); - } - - free(filename_tmp); - - return res; -#endif } /** diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 0cbba0a37..984d19bd9 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +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); static void do_compare(const char* name, const char* vals[], unsigned comparisons); +static void do_reset(); int param_main(int argc, char *argv[]) @@ -130,6 +131,10 @@ param_main(int argc, char *argv[]) errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); } } + + if (!strcmp(argv[1], "reset")) { + do_reset(); + } } errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); @@ -402,3 +407,16 @@ do_compare(const char* name, const char* vals[], unsigned comparisons) exit(ret); } + +static void +do_reset() +{ + param_reset_all(); + + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } +} -- cgit v1.2.3 From e5508a1aa07fb4a0e76dfd077c5beaa114f21695 Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 09:41:41 -0700 Subject: Add Gumstix AeroCore device Based on the work of Andrew Smith [1], add board configuration and device drivers to support the Gumstix AeroCore (previously Aerodroid) board [2]. The AeroCore is an autopilot board based on a STM32F427 similar to the FMUv2. [1] https://github.com/smithandrewc/Firmware [2] https://store.gumstix.com/index.php/products/585/ Signed-off-by: Ash Charles --- Images/aerocore.prototype | 12 + makefiles/board_aerocore.mk | 11 + makefiles/config_aerocore_default.mk | 125 +++ nuttx-configs/aerocore/include/board.h | 263 +++++++ nuttx-configs/aerocore/include/nsh_romfsimg.h | 42 + nuttx-configs/aerocore/nsh/Make.defs | 179 +++++ nuttx-configs/aerocore/nsh/appconfig | 42 + nuttx-configs/aerocore/nsh/defconfig | 950 +++++++++++++++++++++++ nuttx-configs/aerocore/nsh/setenv.sh | 67 ++ nuttx-configs/aerocore/scripts/ld.script | 150 ++++ nuttx-configs/aerocore/src/Makefile | 84 ++ nuttx-configs/aerocore/src/empty.c | 4 + src/drivers/boards/aerocore/aerocore_init.c | 282 +++++++ src/drivers/boards/aerocore/aerocore_led.c | 121 +++ src/drivers/boards/aerocore/aerocore_pwm_servo.c | 117 +++ src/drivers/boards/aerocore/aerocore_spi.c | 183 +++++ src/drivers/boards/aerocore/board_config.h | 176 +++++ src/drivers/boards/aerocore/module.mk | 8 + src/drivers/boards/px4fmu-v1/board_config.h | 2 + src/drivers/boards/px4fmu-v2/board_config.h | 2 + src/drivers/drv_gpio.h | 10 +- src/drivers/drv_gps.h | 4 + src/drivers/gps/gps.cpp | 11 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 84 +- src/drivers/stm32/adc/adc.cpp | 4 + src/drivers/stm32/drv_hrt.c | 4 +- src/lib/version/version.h | 4 + src/modules/commander/commander_helper.cpp | 2 +- src/modules/sensors/sensor_params.c | 9 + src/modules/sensors/sensors.cpp | 10 +- src/systemcmds/mtd/mtd.c | 4 + 33 files changed, 2960 insertions(+), 10 deletions(-) create mode 100644 Images/aerocore.prototype create mode 100644 makefiles/board_aerocore.mk create mode 100644 makefiles/config_aerocore_default.mk create mode 100644 nuttx-configs/aerocore/include/board.h create mode 100644 nuttx-configs/aerocore/include/nsh_romfsimg.h create mode 100644 nuttx-configs/aerocore/nsh/Make.defs create mode 100644 nuttx-configs/aerocore/nsh/appconfig create mode 100644 nuttx-configs/aerocore/nsh/defconfig create mode 100755 nuttx-configs/aerocore/nsh/setenv.sh create mode 100644 nuttx-configs/aerocore/scripts/ld.script create mode 100644 nuttx-configs/aerocore/src/Makefile create mode 100644 nuttx-configs/aerocore/src/empty.c create mode 100644 src/drivers/boards/aerocore/aerocore_init.c create mode 100644 src/drivers/boards/aerocore/aerocore_led.c create mode 100644 src/drivers/boards/aerocore/aerocore_pwm_servo.c create mode 100644 src/drivers/boards/aerocore/aerocore_spi.c create mode 100644 src/drivers/boards/aerocore/board_config.h create mode 100644 src/drivers/boards/aerocore/module.mk (limited to 'src/systemcmds') diff --git a/Images/aerocore.prototype b/Images/aerocore.prototype new file mode 100644 index 000000000..8502a0864 --- /dev/null +++ b/Images/aerocore.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 19, + "magic": "AeroCore", + "description": "Firmware for the Gumstix AeroCore board", + "image": "", + "build_time": 0, + "summary": "AEROCORE", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/makefiles/board_aerocore.mk b/makefiles/board_aerocore.mk new file mode 100644 index 000000000..6f4b93266 --- /dev/null +++ b/makefiles/board_aerocore.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the Gumstix AeroCore +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = AEROCORE + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk new file mode 100644 index 000000000..e038500a6 --- /dev/null +++ b/makefiles/config_aerocore_default.mk @@ -0,0 +1,125 @@ +# +# Makefile for the AeroCore *default* configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/boards/aerocore +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/ms5611 +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/fw_att_pos_estimator +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion +MODULES += lib/launchdetection + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# 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 + +BUILTIN_COMMANDS := \ + $(call _B, hello, , 2048, hello_main) \ + $(call _B, i2c, , 2048, i2c_main) diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h new file mode 100644 index 000000000..8705c1bc2 --- /dev/null +++ b/nuttx-configs/aerocore/include/board.h @@ -0,0 +1,263 @@ +/************************************************************************************ + * configs/aerocore/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The AeroCore uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (24,000,000 / 24) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +/* USART1 on PB[6,7]: GPS */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +/* USART2 on PD[5,6]: J5 Breakout */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_CTS 0 // unused +#define GPIO_USART2_RTS 0 // unused + +/* USART3 on PD[8,9]: to DuoVero UART2 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_CTS 0 // unused +#define GPIO_USART3_RTS 0 // unused + +/* UART7 on PE[78]: J7 Breakout */ +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* + * UART8 on PE[0-1]: System Console on Port C of USB (J7) + * No alternate pin config +*/ + +/* USART[1,6] require a RX DMA configuration */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ + +/* PB[10-11]: I2C2 is broken out on J9 header */ +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * SPI + */ +/* PA[4-7] SPI1 broken out on J12 */ +#define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) + +/* PB[12-15]: SPI2 connected to DuoVero SPI1 */ +#define GPIO_SPI2_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */ +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) + +/* PC[10-12]: SPI3 connected to onboard sensors */ +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz) + +/* PE[11-14]: SPI4 connected to FRAM */ +#define GPIO_SPI4_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */ +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/aerocore/include/nsh_romfsimg.h b/nuttx-configs/aerocore/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/aerocore/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs new file mode 100644 index 000000000..c7a1b71bb --- /dev/null +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/aerocore/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -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 + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/aerocore/nsh/appconfig b/nuttx-configs/aerocore/nsh/appconfig new file mode 100644 index 000000000..2850dac06 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/appconfig @@ -0,0 +1,42 @@ +############################################################################ +# configs/aerocore/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig new file mode 100644 index 000000000..8d0bae7b9 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -0,0 +1,950 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +# CONFIG_STM32_I2C1 is not set +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +# CONFIG_STM32_OTGFS is not set +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI4=y +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +# CONFIG_STM32_TIM10 is not set +# CONFIG_STM32_TIM11 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=36 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +CONFIG_RAMTRON_FUJITSU=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_BAUD=115200 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_BAUD=115200 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +CONFIG_EXAMPLES_HELLO=y +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MTDPART is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +CONFIG_SYSTEM_I2CTOOL=y +CONFIG_I2CTOOL_MINBUS=0 +CONFIG_I2CTOOL_MAXBUS=3 +CONFIG_I2CTOOL_MINADDR=0x03 +CONFIG_I2CTOOL_MAXADDR=0x77 +CONFIG_I2CTOOL_MAXREGADDR=0xff +CONFIG_I2CTOOL_DEFFREQ=4000000 + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/aerocore/nsh/setenv.sh b/nuttx-configs/aerocore/nsh/setenv.sh new file mode 100755 index 000000000..2327f38a1 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/aerocore/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/aerocore/scripts/ld.script b/nuttx-configs/aerocore/scripts/ld.script new file mode 100644 index 000000000..968d3127f --- /dev/null +++ b/nuttx-configs/aerocore/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/aerocore/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/aerocore/src/Makefile b/nuttx-configs/aerocore/src/Makefile new file mode 100644 index 000000000..859ba4ab2 --- /dev/null +++ b/nuttx-configs/aerocore/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/aerocore/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/aerocore/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c new file mode 100644 index 000000000..4e3ba2d7e --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * 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 aerocore_init.c + * + * AeroCore-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi3; +static struct spi_dev_s *spi4; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + + /* Configure Sensors on SPI bus #3 */ + spi3 = up_spiinitialize(3); + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: 1MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi3, 10000000); + SPI_SETBITS(spi3, 8); + SPI_SETMODE(spi3, SPIDEV_MODE3); + SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi3, PX4_SPIDEV_BARO, false); + up_udelay(20); + message("[boot] Initialized SPI port 3 (SENSORS)\n"); + + /* Configure FRAM on SPI bus #4 */ + spi4 = up_spiinitialize(4); + if (!spi4) { + message("[boot] FAILED to initialize SPI port 4\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: ~10MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE0); + SPI_SELECT(spi4, SPIDEV_FLASH, false); + message("[boot] Initialized SPI port 4 (FRAM)\n"); + + return OK; +} diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c new file mode 100644 index 000000000..e40d1730c --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 aerocore_led.c + * + * AeroCore LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + stm32_configgpio(GPIO_LED0); + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_off(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, false); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, false); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_toggle(int led) +{ + switch (led) { + case 0: + if (stm32_gpioread(GPIO_LED0)) + stm32_gpiowrite(GPIO_LED0, false); + else + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c new file mode 100644 index 000000000..251eaff7b --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 aerocore_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1500, + } +}; diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c new file mode 100644 index 000000000..e329bd9d1 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * 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 aerocore_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI1_NSS); + stm32_gpiowrite(GPIO_SPI1_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI2_NSS); + stm32_gpiowrite(GPIO_SPI2_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI3 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); +#endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI4_NSS); + stm32_gpiowrite(GPIO_SPI4_NSS, 1); +#endif +} + +#ifdef CONFIG_STM32_SPI1 +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI1_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI2_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI3 +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI4_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h new file mode 100644 index 000000000..a3257f339 --- /dev/null +++ b/src/drivers/boards/aerocore/board_config.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 board_config.h + * + * AeroCore internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* LEDs */ +#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) + +/* Gyro */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) + +/* Accel & Mag */ +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2) + +/* GPS */ +#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4) + +/* SPI3--Sensors */ +#define PX4_SPI_BUS_SENSORS 3 +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +/* Nominal chip selects for devices on SPI bus #3 */ +#define PX4_SPIDEV_ACCEL_MAG 0 +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_BARO 2 + +/* User GPIOs broken out on J11 */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* PWM + * + * Eight PWM outputs are configured. + * + * Pins: + * + * CH1 : PA8 : TIM1_CH1 + * CH2 : PA9 : TIM1_CH2 + * CH3 : PA10 : TIM1_CH3 + * CH4 : PA11 : TIM1_CH4 + * CH5 : PC6 : TIM3_CH1 + * CH6 : PC7 : TIM3_CH2 + * CH7 : PC8 : TIM3_CH3 + * CH8 : PC9 : TIM3_CH4 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1 +#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3 +#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 +#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 +#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer 8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* Tone Alarm (no onboard speaker )*/ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk new file mode 100644 index 000000000..b53fe0a29 --- /dev/null +++ b/src/drivers/boards/aerocore/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for the AeroCore +# + +SRCS = aerocore_init.c \ + aerocore_pwm_servo.c \ + aerocore_spi.c \ + aerocore_led.c diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 02c26b5c0..58273f2d2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -85,6 +85,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define PX4_SPI_BUS_SENSORS 1 + /* * Use these in place of the spi_dev_e enumeration to * select a specific SPI device on SPI1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7cfca7656..c2de1bfba 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -107,6 +107,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define PX4_SPI_BUS_SENSORS 1 + /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f60964c2b..5acd0d343 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,14 @@ #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +/* + * AeroCore GPIO numbers and configuration. + * + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* no GPIO driver on the PX4IOv1 board */ #endif @@ -146,4 +154,4 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) -#endif /* _DRV_GPIO_H */ \ No newline at end of file +#endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 06e3535b3..96669a021 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -46,7 +46,11 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" +#else #define GPS_DEFAULT_UART_PORT "/dev/ttyS3" +#endif #define GPS_DEVICE_PATH "/dev/gps" diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f..337705570 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,8 @@ #include #include +#include + #include "ubx.h" #include "mtk.h" @@ -421,7 +423,14 @@ GPS::task_main() void GPS::cmd_reset() { - //XXX add reset? +#ifdef GPIO_GPS_NRESET + warnx("Toggling GPS reset pin"); + stm32_configgpio(GPIO_GPS_NRESET); + stm32_gpiowrite(GPIO_GPS_NRESET, 0); + usleep(100); + stm32_gpiowrite(GPIO_GPS_NRESET, 1); + warnx("Toggled GPS reset pin"); +#endif } void diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4dee7649b..4ca8b5e42 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1793,7 +1793,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 26216e840..8759d16a1 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -117,7 +117,7 @@ private: device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf) { - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d72ead9b..1b6b136eb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -92,6 +92,7 @@ public: MODE_2PWM, MODE_4PWM, MODE_6PWM, + MODE_8PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -113,6 +114,9 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + static const unsigned _max_actuators = 8; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -203,6 +207,21 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + /* AeroCore breaks out User GPIOs on J11 */ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, +// {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -382,6 +401,20 @@ PX4FMU::set_mode(Mode mode) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs + debug("MODE_8PWM"); + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xff); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; +#endif + case MODE_NONE: debug("MODE_NONE"); @@ -602,6 +635,9 @@ PX4FMU::task_main() num_outputs = 6; break; + case MODE_8PWM: + num_outputs = 8; + break; default: num_outputs = 0; break; @@ -757,6 +793,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_2PWM: case MODE_4PWM: case MODE_6PWM: +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: +#endif ret = pwm_ioctl(filp, cmd, arg); break; @@ -986,6 +1025,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): + case PWM_SERVO_SET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1013,6 +1061,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { @@ -1040,12 +1097,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(3): case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET_RATEGROUP(6): + case PWM_SERVO_GET_RATEGROUP(7): +#endif *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: + *(unsigned *)arg = 8; + break; +#endif + case MODE_6PWM: *(unsigned *)arg = 6; break; @@ -1091,6 +1158,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_6PWM); break; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + case 8: + set_mode(MODE_8PWM); + break; +#endif default: ret = -EINVAL; @@ -1181,10 +1253,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) unsigned count = len / 2; uint16_t values[6]; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { + // we have at most 8 outputs + count = 8; + } +#else if (count > 6) { // we have at most 6 outputs count = 6; } +#endif // allow for misaligned values memcpy(values, buffer, count * 2); @@ -1458,6 +1537,9 @@ fmu_new_mode(PortMode new_mode) #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + servo_mode = PX4FMU::MODE_8PWM; #endif break; @@ -1776,7 +1858,7 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 3a60d2cae..de13b8969 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -419,6 +419,10 @@ adc_main(int argc, char *argv[]) g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE + /* XXX this hardcodes the default channel set for AeroCore - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 5bb550279..281f918d7 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -141,7 +141,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP -# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN # if CONFIG_STM32_TIM10 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 # endif @@ -150,7 +150,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN # if CONFIG_STM32_TIM11 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif diff --git a/src/lib/version/version.h b/src/lib/version/version.h index af733aaf0..d8ccb6774 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,4 +59,8 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define HW_ARCH "AEROCORE" +#endif + #endif /* VERSION_H_ */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 86f77cdb8..940a04aa1 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -206,7 +206,7 @@ int led_init() return ERROR; } - /* the blue LED is only available on FMUv1 but not FMUv2 */ + /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); /* we consider the amber led mandatory */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 07be3560a..c021e1fde 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#elif CONFIG_ARCH_BOARD_AEROCORE +/** + * Scaling factor for battery voltage sensor on AeroCore. + * + * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); #else /** * Scaling factor for battery voltage sensor on FMU v1. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 18bf97f8d..d31016111 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -126,6 +126,12 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + #define BATT_V_LOWPASS 0.001f #define BATT_V_IGNORE_THRESHOLD 3.5f @@ -797,7 +803,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -806,7 +812,7 @@ Sensors::accel_init() ioctl(fd, SENSORIOCSPOLLRATE, 800); #else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE #endif diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0a88d40f3..a57eaafe7 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -160,7 +160,11 @@ static void ramtron_attach(void) { /* find the right spi */ +#ifdef CONFIG_ARCH_BOARD_AEROCORE + struct spi_dev_s *spi = up_spiinitialize(4); +#else struct spi_dev_s *spi = up_spiinitialize(2); +#endif /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); -- cgit v1.2.3 From 15eaa3aed499941599c28e79dd40281a2de739c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:00:51 +0200 Subject: eeprom driver: Use less excessive perf counters on EEPROM --- src/systemcmds/mtd/24xxxx_mtd.c | 36 +++++++++++++++--------------------- 1 file changed, 15 insertions(+), 21 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index e34be44e3..a3f9dffff 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -142,12 +142,9 @@ struct at24c_dev_s { uint16_t pagesize; /* 32, 63 */ uint16_t npages; /* 128, 256, 512, 1024 */ - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; + perf_counter_t perf_transfers; + perf_counter_t perf_resets_retries; + perf_counter_t perf_errors; }; /************************************************************************************ @@ -298,9 +295,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, for (;;) { - perf_begin(priv->perf_reads); + perf_begin(priv->perf_transfers); ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); + perf_end(priv->perf_transfers); if (ret >= 0) break; @@ -314,10 +311,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, * XXX maybe do special first-read handling with optional * bus reset as well? */ - perf_count(priv->perf_read_retries); + perf_count(priv->perf_resets_retries); if (--tries == 0) { - perf_count(priv->perf_read_errors); + perf_count(priv->perf_errors); return ERROR; } } @@ -383,9 +380,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t for (;;) { - perf_begin(priv->perf_writes); + perf_begin(priv->perf_transfers); ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); + perf_end(priv->perf_transfers); if (ret >= 0) break; @@ -397,7 +394,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t * poll for write completion. */ if (--tries == 0) { - perf_count(priv->perf_write_errors); + perf_count(priv->perf_errors); return ERROR; } } @@ -521,12 +518,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { priv->mtd.ioctl = at24c_ioctl; priv->dev = dev; - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans"); + priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst"); + priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs"); } /* attempt to read to validate device is present */ @@ -548,9 +542,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { } }; - perf_begin(priv->perf_reads); + perf_begin(priv->perf_transfers); int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); + perf_end(priv->perf_transfers); if (ret < 0) { return NULL; -- cgit v1.2.3 From 6018ffa462ad3e78d3cc4514b1a6cff5cf83961f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:02:01 +0200 Subject: nshterm: Use only the stack we really need --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a48588535..fe79ec3b7 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 3000 +MODULE_STACKSIZE = 1500 -- cgit v1.2.3 From a1aa8e84ffc4fb5d8f51df09a0cb70c569cd8760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:04:58 +0200 Subject: Reduce top stack usage --- src/systemcmds/top/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk index 9239aafc3..985ea846f 100644 --- a/src/systemcmds/top/module.mk +++ b/src/systemcmds/top/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = top SRCS = top.c -MODULE_STACKSIZE = 3000 +MODULE_STACKSIZE = 2048 MAXOPTIMIZATION = -Os -- cgit v1.2.3 From c646a8ff1d6fd0b0f08ed317619c20b83b49e418 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:23:16 +0200 Subject: Let the param set command default to non-failing, because unknown params are in the script default init not a reason to give up on the complete boot --- src/systemcmds/param/param.c | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 984d19bd9..d92ee88ef 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -61,7 +61,7 @@ static void do_load(const char* param_file_name); static void do_import(const char* param_file_name); 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); +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(); @@ -117,10 +117,17 @@ param_main(int argc, char *argv[]) } if (!strcmp(argv[1], "set")) { - if (argc >= 4) { - do_set(argv[2], argv[3]); + if (argc >= 5) { + + /* if the fail switch is provided, fails the command if not found */ + bool fail = !strcmp(argv[4], "fail"); + + do_set(argv[2], argv[3], fail); + + } else if (argc >= 4) { + do_set(argv[2], argv[3], false); } else { - errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); + errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'"); } } @@ -282,7 +289,7 @@ do_show_print(void *arg, param_t param) } static void -do_set(const char* name, const char* val) +do_set(const char* name, const char* val, bool fail_on_not_found) { int32_t i; float f; @@ -290,8 +297,8 @@ do_set(const char* name, const char* val) /* set nothing if parameter cannot be found */ if (param == PARAM_INVALID) { - /* param not found */ - errx(1, "Error: Parameter %s not found.", name); + /* param not found - fail silenty in scripts as it prevents booting */ + errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name); } printf("%c %s: ", -- cgit v1.2.3 From 972cf54c9637f9957f542ba7ca975fe449787946 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:36:37 +0200 Subject: Reduce stack size of PWM tool --- src/systemcmds/pwm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk index 4a23bba90..13a24150f 100644 --- a/src/systemcmds/pwm/module.mk +++ b/src/systemcmds/pwm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = pwm SRCS = pwm.c -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 1800 -- cgit v1.2.3 From 8962c272749108af4163e985109d9e760f0dd0f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:37:06 +0200 Subject: Reduce stack size of perf tool --- src/systemcmds/perf/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/systemcmds') diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk index 77952842b..ec39a7a85 100644 --- a/src/systemcmds/perf/module.mk +++ b/src/systemcmds/perf/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = perf SRCS = perf.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1800 -- cgit v1.2.3 From d50ae8bb59635782f35f85f759a25d6f80c40663 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:37:27 +0200 Subject: Reduce stack size of preflight check tool --- src/systemcmds/preflight_check/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/systemcmds') diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk index 7c3c88783..0cb2a4cd0 100644 --- a/src/systemcmds/preflight_check/module.mk +++ b/src/systemcmds/preflight_check/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = preflight_check SRCS = preflight_check.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1800 -- cgit v1.2.3 From b2945fda53839fb70678ea6a1ff286db36ebee8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:03:05 +0200 Subject: param command: Reduce stack size to 1800 after careful testing --- src/systemcmds/param/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk index 63f15ad28..f716eb71e 100644 --- a/src/systemcmds/param/module.mk +++ b/src/systemcmds/param/module.mk @@ -38,7 +38,8 @@ MODULE_COMMAND = param SRCS = param.c -MODULE_STACKSIZE = 4096 +# Note: measurements yielded a max of 900 bytes used. +MODULE_STACKSIZE = 1800 MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 35e7e375de30c3ce0f830cd99ec22f5250e6d95d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:03:45 +0200 Subject: reboot command: Reduce stack size to 800 - it really just reboots and does not need stack --- src/systemcmds/reboot/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/systemcmds') diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk index 19f64af54..edf9d8b37 100644 --- a/src/systemcmds/reboot/module.mk +++ b/src/systemcmds/reboot/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = reboot SRCS = reboot.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 800 -- cgit v1.2.3 From 9f097c1858d002eecbcda8cf8272fb8fbde1a31e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:11:32 +0200 Subject: top: Reduce stack size, but leave some room if it has to print a few more apps --- src/systemcmds/top/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk index 985ea846f..548a09f85 100644 --- a/src/systemcmds/top/module.mk +++ b/src/systemcmds/top/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = top SRCS = top.c -MODULE_STACKSIZE = 2048 +MODULE_STACKSIZE = 1700 MAXOPTIMIZATION = -Os -- cgit v1.2.3 From ea99fd84464ba6141f182dece0ae7a60c3ea9377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:48:41 +0200 Subject: nshterm: Reduce stack size further after more thorough inspection --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index fe79ec3b7..b22b446da 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1500 +MODULE_STACKSIZE = 1200 -- 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') 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 b0d06d21099decf0537a07922ac8dea77083c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 00:13:49 +0200 Subject: config cmd: Eleminate warnings --- src/systemcmds/config/config.c | 42 +++++++++++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 13 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index e6d4b763b..e8ccbc149 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Author: Julian Oes + * Copyright (c) 2012-2014 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 @@ -94,7 +92,6 @@ do_device(int argc, char *argv[]) } int fd; - int ret; fd = open(argv[0], 0); @@ -104,6 +101,8 @@ do_device(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[1], "block")) { /* disable the device publications */ @@ -132,7 +131,6 @@ static void do_gyro(int argc, char *argv[]) { int fd; - int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -142,6 +140,8 @@ do_gyro(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ @@ -173,8 +173,12 @@ do_gyro(int argc, char *argv[]) warnx("gyro self test FAILED! Check calibration:"); struct gyro_scale scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + if (ret) { + err(1, "gyro scale fail"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("gyro calibration and self test OK"); } @@ -199,7 +203,6 @@ static void do_mag(int argc, char *argv[]) { int fd; - int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -209,6 +212,8 @@ do_mag(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ @@ -240,8 +245,13 @@ do_mag(int argc, char *argv[]) warnx("mag self test FAILED! Check calibration:"); struct mag_scale scale; ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + + if (ret) { + errx(ret, "failed getting mag scale"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("mag calibration and self test OK"); } @@ -266,7 +276,6 @@ static void do_accel(int argc, char *argv[]) { int fd; - int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -276,6 +285,8 @@ do_accel(int argc, char *argv[]) } else { + int ret; + if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ @@ -307,8 +318,13 @@ do_accel(int argc, char *argv[]) warnx("accel self test FAILED! Check calibration:"); struct accel_scale scale; ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + + if (ret) { + errx(ret, "failed getting mag scale"); + } + + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); } else { warnx("accel calibration and self test OK"); } -- 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') 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 b7d1f3f10010949241a30a5208e3e9d93306625a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 May 2014 11:01:28 +0200 Subject: Make error reporting consistent --- src/systemcmds/config/config.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index e8ccbc149..4a97d328c 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -173,8 +173,9 @@ do_gyro(int argc, char *argv[]) warnx("gyro self test FAILED! Check calibration:"); struct gyro_scale scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); + if (ret) { - err(1, "gyro scale fail"); + err(1, "failed getting gyro scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); @@ -247,7 +248,7 @@ do_mag(int argc, char *argv[]) ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); if (ret) { - errx(ret, "failed getting mag scale"); + err(ret, "failed getting mag scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); @@ -320,7 +321,7 @@ do_accel(int argc, char *argv[]) ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); if (ret) { - errx(ret, "failed getting mag scale"); + err(ret, "failed getting accel scale"); } warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); -- 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') 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') 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') 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') 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 04efee0bc8a2b0e8e9ffae4cf2c5b28bb1ba9ba9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:23:38 +0200 Subject: nshterm: Hotfix for retries counter --- src/systemcmds/nshterm/nshterm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/systemcmds') diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 7d9484d3e..fca1798e6 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[]) printf("Usage: nshterm \n"); exit(1); } - uint8_t retries = 0; + unsigned retries = 0; int fd = -1; /* try the first 30 seconds */ -- cgit v1.2.3 From d0b78aa8a09cd4259258ab10b77c98e907c3b460 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 13:42:32 +0200 Subject: Add param command which does not reset the autostart params to not hamper auto-config --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- src/systemcmds/param/param.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 30 insertions(+), 2 deletions(-) (limited to 'src/systemcmds') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5856ba84f..1c9ded6a8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -134,8 +134,8 @@ then then # We can't be sure the defaults haven't changed, so # if someone requests a re-configuration, we do it - # cleanly from scratch - param reset + # cleanly from scratch (except autostart / autoconfig) + param reset_nostart set DO_AUTOCONFIG yes else set DO_AUTOCONFIG no diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 328205e29..f8bff2f6f 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -64,6 +64,7 @@ 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(void); +static void do_reset_nostart(void); int param_main(int argc, char *argv[]) @@ -142,6 +143,10 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "reset")) { do_reset(); } + + if (!strcmp(argv[1], "reset_nostart")) { + do_reset_nostart(); + } } errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); @@ -427,3 +432,26 @@ do_reset(void) exit(0); } } + +static void +do_reset_nostart(void) +{ + + int32_t autostart; + int32_t autoconfig; + + (void)param_get(param_find("SYS_AUTOSTART"), &autostart); + (void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig); + + param_reset_all(); + + (void)param_set(param_find("SYS_AUTOSTART"), &autostart); + (void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig); + + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } +} -- cgit v1.2.3 From b9299e68d4147845bab9ed99509b3e50b7c94ae1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 13:35:12 -0700 Subject: Compiler warning fixes --- src/drivers/blinkm/blinkm.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++------- src/modules/commander/commander.cpp | 8 ++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 4 ++-- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/systemlib.c | 3 +++ src/systemcmds/param/param.c | 5 +++-- 8 files changed, 33 insertions(+), 28 deletions(-) (limited to 'src/systemcmds') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 92e089217..6b14f5945 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) : safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), - t_led_color({0}), + t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 212e33ff8..230be9281 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -297,12 +297,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // TO DO - this is messed up and won't compile 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 end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float lon_start = (float)lon_now + start_disp_x / 111111.0f; + float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = (float)lon_now + end_disp_x / 111111.0f; + float lat_end = (float)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); @@ -319,7 +319,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..3bf3c857d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -472,7 +472,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); } else { @@ -634,7 +634,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - char *main_states_str[MAIN_STATE_MAX]; + const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; @@ -643,7 +643,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; - char *arming_states_str[ARMING_STATE_MAX]; + const char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; @@ -652,7 +652,7 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - char *nav_states_str[NAVIGATION_STATE_MAX]; + const char *nav_states_str[NAVIGATION_STATE_MAX]; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; 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 000c02e3d..a9556f453 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 @@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..c22e60ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock { public: // methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : SuperBlock(parent, name), - _isAngularLimit(isAngularLimit), + _isAngularLimit(fAngularLimit), _min(this, "MIN"), _max(this, "MAX") {}; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index ccc516f39..7aa2f3a5f 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT __EXPORT struct system_load_s system_load; -extern FAR struct _TCB *sched_gettcb(pid_t pid); +extern FAR struct tcb_s *sched_gettcb(pid_t pid); void cpuload_initialize_once() { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 9fff3eb88..90d8dd77c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -54,6 +54,9 @@ #include "systemlib.h" +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + void systemreset(bool to_bootloader) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index f8bff2f6f..235ab7186 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param) if (!(arg == NULL)) { /* start search */ - char *ss = search_string; - char *pp = p_name; + const char *ss = search_string; + const char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ -- cgit v1.2.3