diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 6b6708a5cc..57ede0b78b 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -49,6 +49,10 @@ MODULES += modules/mc_att_control MODULES += modules/mc_pos_control_multiplatform MODULES += modules/mc_att_control_multiplatform MODULES += modules/land_detector +MODULES += modules/fw_att_control +MODULES += modules/fw_pos_control_l1 + + # # Library modules @@ -67,9 +71,13 @@ MODULES += modules/controllib # MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += lib/launchdetection + # # POSIX port diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 95ccf4130f..7bf4129e48 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate() float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { float airspeed_result = airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b1304b34bf..d04fe38ac3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -62,10 +62,10 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.airspeed))) { + if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; @@ -94,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 5935f15ceb..9d40c73f3b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController() float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { + if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch) && - isfinite(ctl_data.roll_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d1b6f1bfeb..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.speed_body_u) && - isfinite(ctl_data.speed_body_v) && - isfinite(ctl_data.speed_body_w) && - isfinite(ctl_data.roll_rate_setpoint) && - isfinite(ctl_data.pitch_rate_setpoint))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.speed_body_u) && + PX4_ISFINITE(ctl_data.speed_body_v) && + PX4_ISFINITE(ctl_data.speed_body_w) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control // counter++; - if (!isfinite(_rate_setpoint)) { + if (!PX4_ISFINITE(_rate_setpoint)) { warnx("yaw rate sepoint not finite"); _rate_setpoint = 0.0f; } @@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl /* input conditioning */ float airspeed = ctl_data.airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 7607f71c16..ec2d64ad83 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -98,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< // Only required if airspeed is being measured and controlled float temp = 0; - if (isfinite(airspeed) && airspeed_sensor_enabled()) { + if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { // Get DCM // Calculate speed rate of change // XXX check @@ -142,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, // Get airspeed or default to halfway between min and max if // airspeed is not being used and set speed rate to zero - if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { // If no airspeed available use average of min and max _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); @@ -228,12 +228,12 @@ void TECS::_update_speed_demand(void) void TECS::_update_height_demand(float demand, float state) { // Handle initialization - if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { + if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { _hgt_dem_in_old = demand; } // Apply 2 point moving average to demanded height // This is required because height demand is updated in steps - if (isfinite(demand)) { + if (PX4_ISFINITE(demand)) { _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); } else { _hgt_dem = _hgt_dem_in_old; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d073289952..78ee9812bf 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -42,6 +42,9 @@ */ #include +#include +#include +#include #include #include #include @@ -421,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -800,7 +803,7 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (isfinite(_manual.flaps)) { + if (PX4_ISFINITE(_manual.flaps)) { flaps_control = _manual.flaps; } @@ -810,7 +813,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { @@ -990,7 +993,7 @@ FixedwingAttitudeControl::task_main() control_input.lock_integrator = lock_integrator; /* Run attitude controllers */ - if (isfinite(roll_sp) && isfinite(pitch_sp)) { + if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude @@ -1002,8 +1005,8 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; - if (!isfinite(roll_u)) { + _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1013,8 +1016,8 @@ FixedwingAttitudeControl::task_main() } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; - if (!isfinite(pitch_u)) { + _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1034,11 +1037,11 @@ FixedwingAttitudeControl::task_main() } float yaw_u = _yaw_ctrl.control_bodyrate(control_input); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; - if (!isfinite(yaw_u)) { + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1048,11 +1051,11 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ - _actuators.control[3] = (isfinite(throttle_sp) && + _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; - if (!isfinite(throttle_sp)) { + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } @@ -1145,7 +1148,7 @@ FixedwingAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1159,23 +1162,23 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_att_control {start|stop|status}"); + warnx("usage: fw_att_control {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (att_control::g_control != nullptr) - errx(1, "already running"); + warnx("already running"); att_control::g_control = new FixedwingAttitudeControl; if (att_control::g_control == nullptr) - errx(1, "alloc failed"); + warnx("alloc failed"); if (OK != att_control::g_control->start()) { delete att_control::g_control; att_control::g_control = nullptr; - err(1, "start failed"); + warn("start failed"); } /* check if the waiting is necessary at all */ @@ -1194,7 +1197,7 @@ int fw_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (att_control::g_control == nullptr) - errx(1, "not running"); + warnx("not running"); delete att_control::g_control; att_control::g_control = nullptr; @@ -1203,10 +1206,10 @@ int fw_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (att_control::g_control) { - errx(0, "running"); + warnx("running"); } else { - errx(1, "not running"); + warnx("not running"); } } 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 82326e8c1d..68d5486fa5 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 @@ -52,6 +52,9 @@ */ #include +#include +#include +#include #include #include #include @@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (!isfinite(global_pos.terrain_alt)) { + if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; } @@ -1958,7 +1961,7 @@ FixedwingPositionControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingPositionControl::task_main_trampoline, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1972,16 +1975,16 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + warnx("usage: fw_pos_control_l1 {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (l1_control::g_control != nullptr) - errx(1, "already running"); + warnx("already running"); if (OK != FixedwingPositionControl::start()) { - err(1, "start failed"); + warn("start failed"); } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1997,7 +2000,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (l1_control::g_control == nullptr) - errx(1, "not running"); + warnx("not running"); delete l1_control::g_control; l1_control::g_control = nullptr; @@ -2006,10 +2009,10 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { - errx(0, "running"); + warnx("running"); } else { - errx(1, "not running"); + warnx("not running"); } }