diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index eccb2b7678..b924d62bc8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -5,10 +5,9 @@ echo "[HIL] starting.." -uorb start - # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyS1 +sleep 2 +mavlink start -b 230400 -d /dev/ttyACM0 # Create a fake HIL /dev/pwm_output interface hil mode_pwm @@ -50,7 +49,8 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start +#fw_pos_control_l1 start fw_att_control start echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9264985d6d..ccf2cd47ea 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -21,7 +21,6 @@ if mavlink stop then echo "stopped other MAVLink instance" fi -sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 # Stop commander @@ -37,13 +36,6 @@ then echo "Commander started" fi -# Stop px4io -if px4io stop -then - echo "PX4IO stopped" -fi -sleep 1 - # Start px4io if present if px4io start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c2ef375266..1f466e49f0 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -101,8 +101,14 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1000 + then + sh /etc/init.d/rc.hil + set MODE custom + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi # # Upgrade PX4IO firmware @@ -202,7 +208,6 @@ then then # Telemetry port is on both FMU boards ttyS1 mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 # Start commander commander start diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c88abe59a8..78d1d3e63f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -487,25 +487,27 @@ PX4IO::detect() { int ret; - ASSERT(_task == -1); + if (_task == -1) { - /* do regular cdev init */ - ret = CDev::init(); - if (ret != OK) - return ret; + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; - /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); - if (protocol != PX4IO_PROTOCOL_VERSION) { - if (protocol == _io_reg_get_error) { - log("IO not installed"); - } else { - log("IO version error"); - mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + if (protocol == _io_reg_get_error) { + log("IO not installed"); + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + + return -1; } - - return -1; } + log("IO found"); return 0; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0faba287d8..77ec15c53d 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -38,20 +38,87 @@ */ #include "ecl_pitch_controller.h" +#include +#include +#include +#include +#include +#include ECL_PitchController::ECL_PitchController() : _last_run(0), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, - bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { - return 0.0f; + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = dt_micros / 1000000; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* calculate the offset in the rate resulting from rolling */ + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll)) * _roll_ff; + + float pitch_error = pitch_setpoint - pitch; + /* rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* add turn offset */ + _rate_setpoint += turn_offset; + + _rate_error = _rate_setpoint - pitch_rate; + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 391f90b9df..02ca62dada 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -49,7 +49,7 @@ public: ECL_PitchController(); float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, - bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); @@ -83,7 +83,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: @@ -100,7 +100,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b28ecdabee..6de30fc339 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -39,6 +39,11 @@ #include "../ecl.h" #include "ecl_roll_controller.h" +#include +#include +#include +#include +#include ECL_RollController::ECL_RollController() : _last_run(0), @@ -46,13 +51,14 @@ ECL_RollController::ECL_RollController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _desired_rate(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) { } float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, - float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -60,10 +66,56 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + float roll_error = roll_setpoint - roll; + _rate_setpoint = roll_error / _tc; - return 0.0f; + /* limit the rate */ + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + _rate_error = _rate_setpoint - roll_rate; + + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index bba9ae1634..7fbcdf4b86 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -49,7 +49,7 @@ public: ECL_RollController(); float control(float roll_setpoint, float roll, float roll_rate, - float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f)); + float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); @@ -79,7 +79,7 @@ public: } float get_desired_rate() { - return _desired_rate; + return _rate_setpoint; } private: @@ -93,7 +93,8 @@ private: float _last_output; float _integrator; float _rate_error; - float _desired_rate; + float _rate_setpoint; + float _max_deflection_rad; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0d8a0513f0..a0f901e71b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -38,6 +38,11 @@ */ #include "ecl_yaw_controller.h" +#include +#include +#include +#include +#include ECL_YawController::ECL_YawController() : _last_run(0), @@ -54,6 +59,12 @@ ECL_YawController::ECL_YawController() : float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed) { + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + return 0.0f; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 333fe30ae2..5eeb018fd5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -144,8 +144,8 @@ static int mavlink_fd; /* flags */ static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static unsigned int leds_counter; @@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("commander already running\n"); + warnx("commander already running"); /* this is not an error */ exit(0); } @@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[]) 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + if (!thread_running) + errx(0, "commander already stopped"); + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("\tcommander is running\n"); + warnx("\tcommander is running"); print_status(); } else { - warnx("\tcommander not started\n"); + warnx("\tcommander not started"); } exit(0); @@ -326,6 +343,9 @@ void print_status() warnx("arming: %s", armed_str); } +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -339,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ @@ -526,7 +550,6 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value @@ -538,7 +561,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -595,16 +618,20 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] started"); + int ret; + pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); /* Start monitoring loop */ unsigned counter = 0; @@ -1200,7 +1227,12 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); + ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { + warn("join failed", ret); + } + + rgbled_set_mode(RGBLED_MODE_OFF); /* close fds */ led_deinit(); @@ -1218,9 +1250,6 @@ int commander_thread_main(int argc, char *argv[]) close(param_changed_sub); close(battery_sub); - warnx("exiting"); - fflush(stdout); - thread_running = false; return 0; @@ -1628,7 +1657,7 @@ void *commander_low_prio_loop(void *arg) while (!thread_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) @@ -1785,5 +1814,5 @@ void *commander_low_prio_loop(void *arg) close(cmd_sub); - return 0; + return NULL; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 316b06127b..8ce31550f9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -504,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + // XXX also set lockdown here + ret = OK; } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5eb7cba9b2..cbcd4adfb8 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[]) thread_running = false; - exit(0); + return 0; } static void @@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "mavlink already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink", @@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[]) 2048, mavlink_thread_main, (const char **)argv); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "mavlink already stopped"); + thread_should_exit = true; while (thread_running) { usleep(200000); - printf("."); + warnx("."); } warnx("terminated."); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4674f7a248..222d1f45f2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -755,5 +755,7 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + + pthread_attr_destroy(&receiveloop_attr); return thread; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 53d86ec005..d11a67fc6f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -829,5 +829,7 @@ uorb_receive_start(void) pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + + pthread_attr_destroy(&uorb_attr); return thread; }