diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 78b40d3e87..48de2636e0 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -34,6 +34,7 @@ MODULES += modules/ekf_att_pos_estimator # # Vehicle Control # +MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control @@ -47,6 +48,7 @@ MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator MODULES += modules/commander +MODULES += modules/controllib # # Libraries diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 65600190b2..979b9541be 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 7c520219f1..4f651777ba 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -61,16 +61,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, + (double)pos.lat / (double)1e7, + (double)pos.lon / (double)1e7, missionCmd.lat, missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, + (double)pos.lat / (double)1e7, + (double)pos.lon / (double)1e7, lastMissionCmd.lat, lastMissionCmd.lon, missionCmd.lat, diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a68f4de012..8b4e889abf 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -533,7 +533,7 @@ Mission::heading_sp_update() /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || - !isfinite(_on_arrival_yaw)) { + !PX4_ISFINITE(_on_arrival_yaw)) { return; } @@ -581,7 +581,7 @@ Mission::altitude_sp_foh_update() /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || - !isfinite(_mission_item_previous_alt)) { + !PX4_ISFINITE(_mission_item_previous_alt)) { return; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index dce7d45171..077404b574 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -80,8 +80,8 @@ MissionBlock::is_mission_item_reached() } /* TODO: count turns */ - if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + if (/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { return false; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6a98a7aab1..c104f8162c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -44,6 +44,9 @@ */ #include +#include +#include +#include #include #include @@ -174,7 +177,7 @@ Navigator::~Navigator() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_navigator_task); + px4_task_delete(_navigator_task); break; } } while (_navigator_task != -1); @@ -505,7 +508,7 @@ Navigator::task_main() warnx("exiting."); _navigator_task = -1; - _exit(0); + return; } int @@ -516,9 +519,9 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 20, + SCHED_PRIORITY_MAX -5, 1700, - (main_t)&Navigator::task_main_trampoline, + (px4_main_t)&Navigator::task_main_trampoline, nullptr); if (_navigator_task < 0) { @@ -584,54 +587,57 @@ void Navigator::load_fence_from_file(const char *filename) static void usage() { - errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); + warnx("usage: navigator {start|stop|status|fence|fencefile}"); } int navigator_main(int argc, char *argv[]) { if (argc < 2) { usage(); + return 1; } if (!strcmp(argv[1], "start")) { if (navigator::g_navigator != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } navigator::g_navigator = new Navigator; if (navigator::g_navigator == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != navigator::g_navigator->start()) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } return 0; } - if (navigator::g_navigator == nullptr) - errx(1, "not running"); + if (navigator::g_navigator == nullptr) { + warnx("not running"); + return 1; + } if (!strcmp(argv[1], "stop")) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - } else if (!strcmp(argv[1], "status")) { navigator::g_navigator->status(); - } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "fencefile")) { navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); - } else { usage(); + return 1; } return 0; diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index db54f8c53e..1e3939bb7c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -146,6 +146,8 @@ __END_DECLS #define OK 0 #define ERROR -1 +#define MAX_RAND 32767 + #if defined(__PX4_QURT) #define M_PI 3.14159265358979323846 #define M_PI_2 1.57079632679489661923 @@ -209,6 +211,7 @@ __END_DECLS #define fminf(x, y) ((x) > (y) ? y : x) #endif + #endif /*